Linux 2.6.31 Release for Digi ConnectCore Wi-i.MX boards2.6.31-digi-201102101717

162 files changed, 8427 insertions, 5406 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index 48bbdbe43e69..5dacbbd42483 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -90,6 +90,8 @@ source "drivers/memstick/Kconfig"
 
 source "drivers/leds/Kconfig"
 
+source "drivers/switch/Kconfig"
+
 source "drivers/accessibility/Kconfig"
 
 source "drivers/infiniband/Kconfig"
diff --git a/drivers/Makefile b/drivers/Makefile
index 4a9a1c55b598..c19a0c18e5ed 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -95,6 +95,7 @@ obj-$(CONFIG_ARCH_MXC)		+= mxc/
 obj-$(CONFIG_MMC)		+= mmc/
 obj-$(CONFIG_MEMSTICK)		+= memstick/
 obj-$(CONFIG_NEW_LEDS)		+= leds/
+obj-$(CONFIG_SWITCH)		+= switch/
 obj-$(CONFIG_INFINIBAND)	+= infiniband/
 obj-$(CONFIG_SGI_SN)		+= sn/
 obj-y				+= firmware/
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index ae02b4114a6f..d7198890cdd3 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -55,6 +55,14 @@ config SATA_AHCI
 
 	  If unsure, say N.
 
+config SATA_AHCI_PLATFORM
+	tristate "Platform AHCI SATA support"
+	help
+	  This option enables support for Platform AHCI Serial ATA
+	  controllers.
+
+	  If unsure, say N.
+
 config SATA_SIL24
 	tristate "Silicon Image 3124/3132 SATA support"
 	depends on PCI
@@ -72,6 +80,7 @@ config SATA_FSL
 
 	  If unsure, say N.
 
+
 config ATA_SFF
 	bool "ATA SFF support"
 	default y
@@ -759,6 +768,14 @@ config PATA_FSL
          ATA interface.
          If you are unsure, say N to this.
 
+config PATA_FSL_DISABLE_DMA
+	bool "Disable DMA on Freescale on-chip PATA devices"
+	depends on PATA_FSL
+	default y
+	help
+	  Say yes to disable the Ultra DMA and Multi Word DMA transfers on
+	  Freescale PATA SoC interface.
+
 endif # ATA_SFF
 
 endif # ATA
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index de9fda120642..69bcefa0df6b 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -1,7 +1,8 @@
 
 obj-$(CONFIG_ATA)		+= libata.o
 
-obj-$(CONFIG_SATA_AHCI)		+= ahci.o
+obj-$(CONFIG_SATA_AHCI)		+= ahci.o libahci.o
+obj-$(CONFIG_SATA_AHCI_PLATFORM) += ahci_platform.o libahci.o
 obj-$(CONFIG_SATA_SVW)		+= sata_svw.o
 obj-$(CONFIG_ATA_PIIX)		+= ata_piix.o
 obj-$(CONFIG_SATA_PROMISE)	+= sata_promise.o
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index fe3eba5d6b3e..edc9b0ec9f39 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -45,64 +45,13 @@
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_cmnd.h>
 #include <linux/libata.h>
+#include "ahci.h"
 
 #define DRV_NAME	"ahci"
 #define DRV_VERSION	"3.0"
 
-/* Enclosure Management Control */
-#define EM_CTRL_MSG_TYPE              0x000f0000
-
-/* Enclosure Management LED Message Type */
-#define EM_MSG_LED_HBA_PORT           0x0000000f
-#define EM_MSG_LED_PMP_SLOT           0x0000ff00
-#define EM_MSG_LED_VALUE              0xffff0000
-#define EM_MSG_LED_VALUE_ACTIVITY     0x00070000
-#define EM_MSG_LED_VALUE_OFF          0xfff80000
-#define EM_MSG_LED_VALUE_ON           0x00010000
-
-static int ahci_skip_host_reset;
-static int ahci_ignore_sss;
-
-module_param_named(skip_host_reset, ahci_skip_host_reset, int, 0444);
-MODULE_PARM_DESC(skip_host_reset, "skip global host reset (0=don't skip, 1=skip)");
-
-module_param_named(ignore_sss, ahci_ignore_sss, int, 0444);
-MODULE_PARM_DESC(ignore_sss, "Ignore staggered spinup flag (0=don't ignore, 1=ignore)");
-
-static int ahci_enable_alpm(struct ata_port *ap,
-		enum link_pm policy);
-static void ahci_disable_alpm(struct ata_port *ap);
-static ssize_t ahci_led_show(struct ata_port *ap, char *buf);
-static ssize_t ahci_led_store(struct ata_port *ap, const char *buf,
-			      size_t size);
-static ssize_t ahci_transmit_led_message(struct ata_port *ap, u32 state,
-					ssize_t size);
-
 enum {
 	AHCI_PCI_BAR		= 5,
-	AHCI_MAX_PORTS		= 32,
-	AHCI_MAX_SG		= 168, /* hardware max is 64K */
-	AHCI_DMA_BOUNDARY	= 0xffffffff,
-	AHCI_MAX_CMDS		= 32,
-	AHCI_CMD_SZ		= 32,
-	AHCI_CMD_SLOT_SZ	= AHCI_MAX_CMDS * AHCI_CMD_SZ,
-	AHCI_RX_FIS_SZ		= 256,
-	AHCI_CMD_TBL_CDB	= 0x40,
-	AHCI_CMD_TBL_HDR_SZ	= 0x80,
-	AHCI_CMD_TBL_SZ		= AHCI_CMD_TBL_HDR_SZ + (AHCI_MAX_SG * 16),
-	AHCI_CMD_TBL_AR_SZ	= AHCI_CMD_TBL_SZ * AHCI_MAX_CMDS,
-	AHCI_PORT_PRIV_DMA_SZ	= AHCI_CMD_SLOT_SZ + AHCI_CMD_TBL_AR_SZ +
-				  AHCI_RX_FIS_SZ,
-	AHCI_IRQ_ON_SG		= (1 << 31),
-	AHCI_CMD_ATAPI		= (1 << 5),
-	AHCI_CMD_WRITE		= (1 << 6),
-	AHCI_CMD_PREFETCH	= (1 << 7),
-	AHCI_CMD_RESET		= (1 << 8),
-	AHCI_CMD_CLR_BUSY	= (1 << 10),
-
-	RX_FIS_D2H_REG		= 0x40,	/* offset of D2H Register FIS data */
-	RX_FIS_SDB		= 0x58, /* offset of SDB FIS data */
-	RX_FIS_UNK		= 0x60, /* offset of Unknown FIS data */
 
 	board_ahci		= 0,
 	board_ahci_vt8251	= 1,
@@ -113,280 +62,20 @@ enum {
 	board_ahci_mcp65	= 6,
 	board_ahci_nopmp	= 7,
 	board_ahci_yesncq	= 8,
-
-	/* global controller registers */
-	HOST_CAP		= 0x00, /* host capabilities */
-	HOST_CTL		= 0x04, /* global host control */
-	HOST_IRQ_STAT		= 0x08, /* interrupt status */
-	HOST_PORTS_IMPL		= 0x0c, /* bitmap of implemented ports */
-	HOST_VERSION		= 0x10, /* AHCI spec. version compliancy */
-	HOST_EM_LOC		= 0x1c, /* Enclosure Management location */
-	HOST_EM_CTL		= 0x20, /* Enclosure Management Control */
-
-	/* HOST_CTL bits */
-	HOST_RESET		= (1 << 0),  /* reset controller; self-clear */
-	HOST_IRQ_EN		= (1 << 1),  /* global IRQ enable */
-	HOST_AHCI_EN		= (1 << 31), /* AHCI enabled */
-
-	/* HOST_CAP bits */
-	HOST_CAP_EMS		= (1 << 6),  /* Enclosure Management support */
-	HOST_CAP_SSC		= (1 << 14), /* Slumber capable */
-	HOST_CAP_PMP		= (1 << 17), /* Port Multiplier support */
-	HOST_CAP_CLO		= (1 << 24), /* Command List Override support */
-	HOST_CAP_ALPM		= (1 << 26), /* Aggressive Link PM support */
-	HOST_CAP_SSS		= (1 << 27), /* Staggered Spin-up */
-	HOST_CAP_SNTF		= (1 << 29), /* SNotification register */
-	HOST_CAP_NCQ		= (1 << 30), /* Native Command Queueing */
-	HOST_CAP_64		= (1 << 31), /* PCI DAC (64-bit DMA) support */
-
-	/* registers for each SATA port */
-	PORT_LST_ADDR		= 0x00, /* command list DMA addr */
-	PORT_LST_ADDR_HI	= 0x04, /* command list DMA addr hi */
-	PORT_FIS_ADDR		= 0x08, /* FIS rx buf addr */
-	PORT_FIS_ADDR_HI	= 0x0c, /* FIS rx buf addr hi */
-	PORT_IRQ_STAT		= 0x10, /* interrupt status */
-	PORT_IRQ_MASK		= 0x14, /* interrupt enable/disable mask */
-	PORT_CMD		= 0x18, /* port command */
-	PORT_TFDATA		= 0x20,	/* taskfile data */
-	PORT_SIG		= 0x24,	/* device TF signature */
-	PORT_CMD_ISSUE		= 0x38, /* command issue */
-	PORT_SCR_STAT		= 0x28, /* SATA phy register: SStatus */
-	PORT_SCR_CTL		= 0x2c, /* SATA phy register: SControl */
-	PORT_SCR_ERR		= 0x30, /* SATA phy register: SError */
-	PORT_SCR_ACT		= 0x34, /* SATA phy register: SActive */
-	PORT_SCR_NTF		= 0x3c, /* SATA phy register: SNotification */
-
-	/* PORT_IRQ_{STAT,MASK} bits */
-	PORT_IRQ_COLD_PRES	= (1 << 31), /* cold presence detect */
-	PORT_IRQ_TF_ERR		= (1 << 30), /* task file error */
-	PORT_IRQ_HBUS_ERR	= (1 << 29), /* host bus fatal error */
-	PORT_IRQ_HBUS_DATA_ERR	= (1 << 28), /* host bus data error */
-	PORT_IRQ_IF_ERR		= (1 << 27), /* interface fatal error */
-	PORT_IRQ_IF_NONFATAL	= (1 << 26), /* interface non-fatal error */
-	PORT_IRQ_OVERFLOW	= (1 << 24), /* xfer exhausted available S/G */
-	PORT_IRQ_BAD_PMP	= (1 << 23), /* incorrect port multiplier */
-
-	PORT_IRQ_PHYRDY		= (1 << 22), /* PhyRdy changed */
-	PORT_IRQ_DEV_ILCK	= (1 << 7), /* device interlock */
-	PORT_IRQ_CONNECT	= (1 << 6), /* port connect change status */
-	PORT_IRQ_SG_DONE	= (1 << 5), /* descriptor processed */
-	PORT_IRQ_UNK_FIS	= (1 << 4), /* unknown FIS rx'd */
-	PORT_IRQ_SDB_FIS	= (1 << 3), /* Set Device Bits FIS rx'd */
-	PORT_IRQ_DMAS_FIS	= (1 << 2), /* DMA Setup FIS rx'd */
-	PORT_IRQ_PIOS_FIS	= (1 << 1), /* PIO Setup FIS rx'd */
-	PORT_IRQ_D2H_REG_FIS	= (1 << 0), /* D2H Register FIS rx'd */
-
-	PORT_IRQ_FREEZE		= PORT_IRQ_HBUS_ERR |
-				  PORT_IRQ_IF_ERR |
-				  PORT_IRQ_CONNECT |
-				  PORT_IRQ_PHYRDY |
-				  PORT_IRQ_UNK_FIS |
-				  PORT_IRQ_BAD_PMP,
-	PORT_IRQ_ERROR		= PORT_IRQ_FREEZE |
-				  PORT_IRQ_TF_ERR |
-				  PORT_IRQ_HBUS_DATA_ERR,
-	DEF_PORT_IRQ		= PORT_IRQ_ERROR | PORT_IRQ_SG_DONE |
-				  PORT_IRQ_SDB_FIS | PORT_IRQ_DMAS_FIS |
-				  PORT_IRQ_PIOS_FIS | PORT_IRQ_D2H_REG_FIS,
-
-	/* PORT_CMD bits */
-	PORT_CMD_ASP		= (1 << 27), /* Aggressive Slumber/Partial */
-	PORT_CMD_ALPE		= (1 << 26), /* Aggressive Link PM enable */
-	PORT_CMD_ATAPI		= (1 << 24), /* Device is ATAPI */
-	PORT_CMD_PMP		= (1 << 17), /* PMP attached */
-	PORT_CMD_LIST_ON	= (1 << 15), /* cmd list DMA engine running */
-	PORT_CMD_FIS_ON		= (1 << 14), /* FIS DMA engine running */
-	PORT_CMD_FIS_RX		= (1 << 4), /* Enable FIS receive DMA engine */
-	PORT_CMD_CLO		= (1 << 3), /* Command list override */
-	PORT_CMD_POWER_ON	= (1 << 2), /* Power up device */
-	PORT_CMD_SPIN_UP	= (1 << 1), /* Spin up device */
-	PORT_CMD_START		= (1 << 0), /* Enable port DMA engine */
-
-	PORT_CMD_ICC_MASK	= (0xf << 28), /* i/f ICC state mask */
-	PORT_CMD_ICC_ACTIVE	= (0x1 << 28), /* Put i/f in active state */
-	PORT_CMD_ICC_PARTIAL	= (0x2 << 28), /* Put i/f in partial state */
-	PORT_CMD_ICC_SLUMBER	= (0x6 << 28), /* Put i/f in slumber state */
-
-	/* hpriv->flags bits */
-	AHCI_HFLAG_NO_NCQ		= (1 << 0),
-	AHCI_HFLAG_IGN_IRQ_IF_ERR	= (1 << 1), /* ignore IRQ_IF_ERR */
-	AHCI_HFLAG_IGN_SERR_INTERNAL	= (1 << 2), /* ignore SERR_INTERNAL */
-	AHCI_HFLAG_32BIT_ONLY		= (1 << 3), /* force 32bit */
-	AHCI_HFLAG_MV_PATA		= (1 << 4), /* PATA port */
-	AHCI_HFLAG_NO_MSI		= (1 << 5), /* no PCI MSI */
-	AHCI_HFLAG_NO_PMP		= (1 << 6), /* no PMP */
-	AHCI_HFLAG_NO_HOTPLUG		= (1 << 7), /* ignore PxSERR.DIAG.N */
-	AHCI_HFLAG_SECT255		= (1 << 8), /* max 255 sectors */
-	AHCI_HFLAG_YES_NCQ		= (1 << 9), /* force NCQ cap on */
-	AHCI_HFLAG_NO_SUSPEND		= (1 << 10), /* don't suspend */
-	AHCI_HFLAG_SRST_TOUT_IS_OFFLINE	= (1 << 11), /* treat SRST timeout as
-							link offline */
-
-	/* ap->flags bits */
-
-	AHCI_FLAG_COMMON		= ATA_FLAG_SATA | ATA_FLAG_NO_LEGACY |
-					  ATA_FLAG_MMIO | ATA_FLAG_PIO_DMA |
-					  ATA_FLAG_ACPI_SATA | ATA_FLAG_AN |
-					  ATA_FLAG_IPM,
-
-	ICH_MAP				= 0x90, /* ICH MAP register */
-
-	/* em constants */
-	EM_MAX_SLOTS			= 8,
-	EM_MAX_RETRY			= 5,
-
-	/* em_ctl bits */
-	EM_CTL_RST			= (1 << 9), /* Reset */
-	EM_CTL_TM			= (1 << 8), /* Transmit Message */
-	EM_CTL_ALHD			= (1 << 26), /* Activity LED */
-};
-
-struct ahci_cmd_hdr {
-	__le32			opts;
-	__le32			status;
-	__le32			tbl_addr;
-	__le32			tbl_addr_hi;
-	__le32			reserved[4];
-};
-
-struct ahci_sg {
-	__le32			addr;
-	__le32			addr_hi;
-	__le32			reserved;
-	__le32			flags_size;
+	board_ahci_nosntf	= 9,
 };
 
-struct ahci_em_priv {
-	enum sw_activity blink_policy;
-	struct timer_list timer;
-	unsigned long saved_activity;
-	unsigned long activity;
-	unsigned long led_state;
-};
-
-struct ahci_host_priv {
-	unsigned int		flags;		/* AHCI_HFLAG_* */
-	u32			cap;		/* cap to use */
-	u32			port_map;	/* port map to use */
-	u32			saved_cap;	/* saved initial cap */
-	u32			saved_port_map;	/* saved initial port_map */
-	u32 			em_loc; /* enclosure management location */
-};
-
-struct ahci_port_priv {
-	struct ata_link		*active_link;
-	struct ahci_cmd_hdr	*cmd_slot;
-	dma_addr_t		cmd_slot_dma;
-	void			*cmd_tbl;
-	dma_addr_t		cmd_tbl_dma;
-	void			*rx_fis;
-	dma_addr_t		rx_fis_dma;
-	/* for NCQ spurious interrupt analysis */
-	unsigned int		ncq_saw_d2h:1;
-	unsigned int		ncq_saw_dmas:1;
-	unsigned int		ncq_saw_sdb:1;
-	u32 			intr_mask;	/* interrupts to enable */
-	/* enclosure management info per PM slot */
-	struct ahci_em_priv	em_priv[EM_MAX_SLOTS];
-};
-
-static int ahci_scr_read(struct ata_link *link, unsigned int sc_reg, u32 *val);
-static int ahci_scr_write(struct ata_link *link, unsigned int sc_reg, u32 val);
 static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
-static unsigned int ahci_qc_issue(struct ata_queued_cmd *qc);
-static bool ahci_qc_fill_rtf(struct ata_queued_cmd *qc);
-static int ahci_port_start(struct ata_port *ap);
-static void ahci_port_stop(struct ata_port *ap);
-static void ahci_qc_prep(struct ata_queued_cmd *qc);
-static void ahci_freeze(struct ata_port *ap);
-static void ahci_thaw(struct ata_port *ap);
-static void ahci_pmp_attach(struct ata_port *ap);
-static void ahci_pmp_detach(struct ata_port *ap);
-static int ahci_softreset(struct ata_link *link, unsigned int *class,
-			  unsigned long deadline);
 static int ahci_sb600_softreset(struct ata_link *link, unsigned int *class,
 			  unsigned long deadline);
-static int ahci_hardreset(struct ata_link *link, unsigned int *class,
-			  unsigned long deadline);
 static int ahci_vt8251_hardreset(struct ata_link *link, unsigned int *class,
 				 unsigned long deadline);
 static int ahci_p5wdh_hardreset(struct ata_link *link, unsigned int *class,
 				unsigned long deadline);
-static void ahci_postreset(struct ata_link *link, unsigned int *class);
-static void ahci_error_handler(struct ata_port *ap);
-static void ahci_post_internal_cmd(struct ata_queued_cmd *qc);
-static int ahci_port_resume(struct ata_port *ap);
-static void ahci_dev_config(struct ata_device *dev);
-static void ahci_fill_cmd_slot(struct ahci_port_priv *pp, unsigned int tag,
-			       u32 opts);
 #ifdef CONFIG_PM
-static int ahci_port_suspend(struct ata_port *ap, pm_message_t mesg);
 static int ahci_pci_device_suspend(struct pci_dev *pdev, pm_message_t mesg);
 static int ahci_pci_device_resume(struct pci_dev *pdev);
 #endif
-static ssize_t ahci_activity_show(struct ata_device *dev, char *buf);
-static ssize_t ahci_activity_store(struct ata_device *dev,
-				   enum sw_activity val);
-static void ahci_init_sw_activity(struct ata_link *link);
-
-static struct device_attribute *ahci_shost_attrs[] = {
-	&dev_attr_link_power_management_policy,
-	&dev_attr_em_message_type,
-	&dev_attr_em_message,
-	NULL
-};
-
-static struct device_attribute *ahci_sdev_attrs[] = {
-	&dev_attr_sw_activity,
-	&dev_attr_unload_heads,
-	NULL
-};
-
-static struct scsi_host_template ahci_sht = {
-	ATA_NCQ_SHT(DRV_NAME),
-	.can_queue		= AHCI_MAX_CMDS - 1,
-	.sg_tablesize		= AHCI_MAX_SG,
-	.dma_boundary		= AHCI_DMA_BOUNDARY,
-	.shost_attrs		= ahci_shost_attrs,
-	.sdev_attrs		= ahci_sdev_attrs,
-};
-
-static struct ata_port_operations ahci_ops = {
-	.inherits		= &sata_pmp_port_ops,
-
-	.qc_defer		= sata_pmp_qc_defer_cmd_switch,
-	.qc_prep		= ahci_qc_prep,
-	.qc_issue		= ahci_qc_issue,
-	.qc_fill_rtf		= ahci_qc_fill_rtf,
-
-	.freeze			= ahci_freeze,
-	.thaw			= ahci_thaw,
-	.softreset		= ahci_softreset,
-	.hardreset		= ahci_hardreset,
-	.postreset		= ahci_postreset,
-	.pmp_softreset		= ahci_softreset,
-	.error_handler		= ahci_error_handler,
-	.post_internal_cmd	= ahci_post_internal_cmd,
-	.dev_config		= ahci_dev_config,
-
-	.scr_read		= ahci_scr_read,
-	.scr_write		= ahci_scr_write,
-	.pmp_attach		= ahci_pmp_attach,
-	.pmp_detach		= ahci_pmp_detach,
-
-	.enable_pm		= ahci_enable_alpm,
-	.disable_pm		= ahci_disable_alpm,
-	.em_show		= ahci_led_show,
-	.em_store		= ahci_led_store,
-	.sw_activity_show	= ahci_activity_show,
-	.sw_activity_store	= ahci_activity_store,
-#ifdef CONFIG_PM
-	.port_suspend		= ahci_port_suspend,
-	.port_resume		= ahci_port_resume,
-#endif
-	.port_start		= ahci_port_start,
-	.port_stop		= ahci_port_stop,
-};
 
 static struct ata_port_operations ahci_vt8251_ops = {
 	.inherits		= &ahci_ops,
@@ -473,7 +162,7 @@ static const struct ata_port_info ahci_port_info[] = {
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
 	},
-	/* board_ahci_yesncq */
+	[board_ahci_yesncq] =
 	{
 		AHCI_HFLAGS	(AHCI_HFLAG_YES_NCQ),
 		.flags		= AHCI_FLAG_COMMON,
@@ -481,6 +170,14 @@ static const struct ata_port_info ahci_port_info[] = {
 		.udma_mask	= ATA_UDMA6,
 		.port_ops	= &ahci_ops,
 	},
+	[board_ahci_nosntf] =
+	{
+		AHCI_HFLAGS	(AHCI_HFLAG_NO_SNTF),
+		.flags		= AHCI_FLAG_COMMON,
+		.pio_mask	= ATA_PIO4,
+		.udma_mask	= ATA_UDMA6,
+		.port_ops	= &ahci_ops,
+	},
 };
 
 static const struct pci_device_id ahci_pci_tbl[] = {
@@ -496,7 +193,7 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	{ PCI_VDEVICE(INTEL, 0x2683), board_ahci }, /* ESB2 */
 	{ PCI_VDEVICE(INTEL, 0x27c6), board_ahci }, /* ICH7-M DH */
 	{ PCI_VDEVICE(INTEL, 0x2821), board_ahci }, /* ICH8 */
-	{ PCI_VDEVICE(INTEL, 0x2822), board_ahci }, /* ICH8 */
+	{ PCI_VDEVICE(INTEL, 0x2822), board_ahci_nosntf }, /* ICH8 */
 	{ PCI_VDEVICE(INTEL, 0x2824), board_ahci }, /* ICH8 */
 	{ PCI_VDEVICE(INTEL, 0x2829), board_ahci }, /* ICH8M */
 	{ PCI_VDEVICE(INTEL, 0x282a), board_ahci }, /* ICH8M */
@@ -644,12 +341,6 @@ static struct pci_driver ahci_pci_driver = {
 #endif
 };
 
-static int ahci_em_messages = 1;
-module_param(ahci_em_messages, int, 0444);
-/* add other LED protocol types when they become supported */
-MODULE_PARM_DESC(ahci_em_messages,
-	"Set AHCI Enclosure Management Message type (0 = disabled, 1 = LED");
-
 #if defined(CONFIG_PATA_MARVELL) || defined(CONFIG_PATA_MARVELL_MODULE)
 static int marvell_enable;
 #else
@@ -659,112 +350,15 @@ module_param(marvell_enable, int, 0644);
 MODULE_PARM_DESC(marvell_enable, "Marvell SATA via AHCI (1 = enabled)");
 
 
-static inline int ahci_nr_ports(u32 cap)
-{
-	return (cap & 0x1f) + 1;
-}
-
-static inline void __iomem *__ahci_port_base(struct ata_host *host,
-					     unsigned int port_no)
-{
-	void __iomem *mmio = host->iomap[AHCI_PCI_BAR];
-
-	return mmio + 0x100 + (port_no * 0x80);
-}
-
-static inline void __iomem *ahci_port_base(struct ata_port *ap)
-{
-	return __ahci_port_base(ap->host, ap->port_no);
-}
-
-static void ahci_enable_ahci(void __iomem *mmio)
-{
-	int i;
-	u32 tmp;
-
-	/* turn on AHCI_EN */
-	tmp = readl(mmio + HOST_CTL);
-	if (tmp & HOST_AHCI_EN)
-		return;
-
-	/* Some controllers need AHCI_EN to be written multiple times.
-	 * Try a few times before giving up.
-	 */
-	for (i = 0; i < 5; i++) {
-		tmp |= HOST_AHCI_EN;
-		writel(tmp, mmio + HOST_CTL);
-		tmp = readl(mmio + HOST_CTL);	/* flush && sanity check */
-		if (tmp & HOST_AHCI_EN)
-			return;
-		msleep(10);
-	}
-
-	WARN_ON(1);
-}
-
-/**
- *	ahci_save_initial_config - Save and fixup initial config values
- *	@pdev: target PCI device
- *	@hpriv: host private area to store config values
- *
- *	Some registers containing configuration info might be setup by
- *	BIOS and might be cleared on reset.  This function saves the
- *	initial values of those registers into @hpriv such that they
- *	can be restored after controller reset.
- *
- *	If inconsistent, config values are fixed up by this function.
- *
- *	LOCKING:
- *	None.
- */
-static void ahci_save_initial_config(struct pci_dev *pdev,
-				     struct ahci_host_priv *hpriv)
+static void ahci_pci_save_initial_config(struct pci_dev *pdev,
+					 struct ahci_host_priv *hpriv)
 {
-	void __iomem *mmio = pcim_iomap_table(pdev)[AHCI_PCI_BAR];
-	u32 cap, port_map;
-	int i;
-	int mv;
-
-	/* make sure AHCI mode is enabled before accessing CAP */
-	ahci_enable_ahci(mmio);
+	unsigned int force_port_map = 0;
+	unsigned int mask_port_map = 0;
 
-	/* Values prefixed with saved_ are written back to host after
-	 * reset.  Values without are used for driver operation.
-	 */
-	hpriv->saved_cap = cap = readl(mmio + HOST_CAP);
-	hpriv->saved_port_map = port_map = readl(mmio + HOST_PORTS_IMPL);
-
-	/* some chips have errata preventing 64bit use */
-	if ((cap & HOST_CAP_64) && (hpriv->flags & AHCI_HFLAG_32BIT_ONLY)) {
-		dev_printk(KERN_INFO, &pdev->dev,
-			   "controller can't do 64bit DMA, forcing 32bit\n");
-		cap &= ~HOST_CAP_64;
-	}
-
-	if ((cap & HOST_CAP_NCQ) && (hpriv->flags & AHCI_HFLAG_NO_NCQ)) {
-		dev_printk(KERN_INFO, &pdev->dev,
-			   "controller can't do NCQ, turning off CAP_NCQ\n");
-		cap &= ~HOST_CAP_NCQ;
-	}
-
-	if (!(cap & HOST_CAP_NCQ) && (hpriv->flags & AHCI_HFLAG_YES_NCQ)) {
-		dev_printk(KERN_INFO, &pdev->dev,
-			   "controller can do NCQ, turning on CAP_NCQ\n");
-		cap |= HOST_CAP_NCQ;
-	}
-
-	if ((cap & HOST_CAP_PMP) && (hpriv->flags & AHCI_HFLAG_NO_PMP)) {
-		dev_printk(KERN_INFO, &pdev->dev,
-			   "controller can't do PMP, turning off CAP_PMP\n");
-		cap &= ~HOST_CAP_PMP;
-	}
-
-	if (pdev->vendor == PCI_VENDOR_ID_JMICRON && pdev->device == 0x2361 &&
-	    port_map != 1) {
-		dev_printk(KERN_INFO, &pdev->dev,
-			   "JMB361 has only one port, port_map 0x%x -> 0x%x\n",
-			   port_map, 1);
-		port_map = 1;
+	if (pdev->vendor == PCI_VENDOR_ID_JMICRON && pdev->device == 0x2361) {
+		dev_info(&pdev->dev, "JMB361 has only one port\n");
+		force_port_map = 1;
 	}
 
 	/*
@@ -774,466 +368,25 @@ static void ahci_save_initial_config(struct pci_dev *pdev,
 	 */
 	if (hpriv->flags & AHCI_HFLAG_MV_PATA) {
 		if (pdev->device == 0x6121)
-			mv = 0x3;
+			mask_port_map = 0x3;
 		else
-			mv = 0xf;
-		dev_printk(KERN_ERR, &pdev->dev,
-			   "MV_AHCI HACK: port_map %x -> %x\n",
-			   port_map,
-			   port_map & mv);
-		dev_printk(KERN_ERR, &pdev->dev,
+			mask_port_map = 0xf;
+		dev_info(&pdev->dev,
 			  "Disabling your PATA port. Use the boot option 'ahci.marvell_enable=0' to avoid this.\n");
-
-		port_map &= mv;
-	}
-
-	/* cross check port_map and cap.n_ports */
-	if (port_map) {
-		int map_ports = 0;
-
-		for (i = 0; i < AHCI_MAX_PORTS; i++)
-			if (port_map & (1 << i))
-				map_ports++;
-
-		/* If PI has more ports than n_ports, whine, clear
-		 * port_map and let it be generated from n_ports.
-		 */
-		if (map_ports > ahci_nr_ports(cap)) {
-			dev_printk(KERN_WARNING, &pdev->dev,
-				   "implemented port map (0x%x) contains more "
-				   "ports than nr_ports (%u), using nr_ports\n",
-				   port_map, ahci_nr_ports(cap));
-			port_map = 0;
-		}
-	}
-
-	/* fabricate port_map from cap.nr_ports */
-	if (!port_map) {
-		port_map = (1 << ahci_nr_ports(cap)) - 1;
-		dev_printk(KERN_WARNING, &pdev->dev,
-			   "forcing PORTS_IMPL to 0x%x\n", port_map);
-
-		/* write the fixed up value to the PI register */
-		hpriv->saved_port_map = port_map;
-	}
-
-	/* record values to use during operation */
-	hpriv->cap = cap;
-	hpriv->port_map = port_map;
-}
-
-/**
- *	ahci_restore_initial_config - Restore initial config
- *	@host: target ATA host
- *
- *	Restore initial config stored by ahci_save_initial_config().
- *
- *	LOCKING:
- *	None.
- */
-static void ahci_restore_initial_config(struct ata_host *host)
-{
-	struct ahci_host_priv *hpriv = host->private_data;
-	void __iomem *mmio = host->iomap[AHCI_PCI_BAR];
-
-	writel(hpriv->saved_cap, mmio + HOST_CAP);
-	writel(hpriv->saved_port_map, mmio + HOST_PORTS_IMPL);
-	(void) readl(mmio + HOST_PORTS_IMPL);	/* flush */
-}
-
-static unsigned ahci_scr_offset(struct ata_port *ap, unsigned int sc_reg)
-{
-	static const int offset[] = {
-		[SCR_STATUS]		= PORT_SCR_STAT,
-		[SCR_CONTROL]		= PORT_SCR_CTL,
-		[SCR_ERROR]		= PORT_SCR_ERR,
-		[SCR_ACTIVE]		= PORT_SCR_ACT,
-		[SCR_NOTIFICATION]	= PORT_SCR_NTF,
-	};
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-
-	if (sc_reg < ARRAY_SIZE(offset) &&
-	    (sc_reg != SCR_NOTIFICATION || (hpriv->cap & HOST_CAP_SNTF)))
-		return offset[sc_reg];
-	return 0;
-}
-
-static int ahci_scr_read(struct ata_link *link, unsigned int sc_reg, u32 *val)
-{
-	void __iomem *port_mmio = ahci_port_base(link->ap);
-	int offset = ahci_scr_offset(link->ap, sc_reg);
-
-	if (offset) {
-		*val = readl(port_mmio + offset);
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static int ahci_scr_write(struct ata_link *link, unsigned int sc_reg, u32 val)
-{
-	void __iomem *port_mmio = ahci_port_base(link->ap);
-	int offset = ahci_scr_offset(link->ap, sc_reg);
-
-	if (offset) {
-		writel(val, port_mmio + offset);
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static void ahci_start_engine(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u32 tmp;
-
-	/* start DMA */
-	tmp = readl(port_mmio + PORT_CMD);
-	tmp |= PORT_CMD_START;
-	writel(tmp, port_mmio + PORT_CMD);
-	readl(port_mmio + PORT_CMD); /* flush */
-}
-
-static int ahci_stop_engine(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u32 tmp;
-
-	tmp = readl(port_mmio + PORT_CMD);
-
-	/* check if the HBA is idle */
-	if ((tmp & (PORT_CMD_START | PORT_CMD_LIST_ON)) == 0)
-		return 0;
-
-	/* setting HBA to idle */
-	tmp &= ~PORT_CMD_START;
-	writel(tmp, port_mmio + PORT_CMD);
-
-	/* wait for engine to stop. This could be as long as 500 msec */
-	tmp = ata_wait_register(port_mmio + PORT_CMD,
-				PORT_CMD_LIST_ON, PORT_CMD_LIST_ON, 1, 500);
-	if (tmp & PORT_CMD_LIST_ON)
-		return -EIO;
-
-	return 0;
-}
-
-static void ahci_start_fis_rx(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	struct ahci_port_priv *pp = ap->private_data;
-	u32 tmp;
-
-	/* set FIS registers */
-	if (hpriv->cap & HOST_CAP_64)
-		writel((pp->cmd_slot_dma >> 16) >> 16,
-		       port_mmio + PORT_LST_ADDR_HI);
-	writel(pp->cmd_slot_dma & 0xffffffff, port_mmio + PORT_LST_ADDR);
-
-	if (hpriv->cap & HOST_CAP_64)
-		writel((pp->rx_fis_dma >> 16) >> 16,
-		       port_mmio + PORT_FIS_ADDR_HI);
-	writel(pp->rx_fis_dma & 0xffffffff, port_mmio + PORT_FIS_ADDR);
-
-	/* enable FIS reception */
-	tmp = readl(port_mmio + PORT_CMD);
-	tmp |= PORT_CMD_FIS_RX;
-	writel(tmp, port_mmio + PORT_CMD);
-
-	/* flush */
-	readl(port_mmio + PORT_CMD);
-}
-
-static int ahci_stop_fis_rx(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u32 tmp;
-
-	/* disable FIS reception */
-	tmp = readl(port_mmio + PORT_CMD);
-	tmp &= ~PORT_CMD_FIS_RX;
-	writel(tmp, port_mmio + PORT_CMD);
-
-	/* wait for completion, spec says 500ms, give it 1000 */
-	tmp = ata_wait_register(port_mmio + PORT_CMD, PORT_CMD_FIS_ON,
-				PORT_CMD_FIS_ON, 10, 1000);
-	if (tmp & PORT_CMD_FIS_ON)
-		return -EBUSY;
-
-	return 0;
-}
-
-static void ahci_power_up(struct ata_port *ap)
-{
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u32 cmd;
-
-	cmd = readl(port_mmio + PORT_CMD) & ~PORT_CMD_ICC_MASK;
-
-	/* spin up device */
-	if (hpriv->cap & HOST_CAP_SSS) {
-		cmd |= PORT_CMD_SPIN_UP;
-		writel(cmd, port_mmio + PORT_CMD);
-	}
-
-	/* wake up link */
-	writel(cmd | PORT_CMD_ICC_ACTIVE, port_mmio + PORT_CMD);
-}
-
-static void ahci_disable_alpm(struct ata_port *ap)
-{
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u32 cmd;
-	struct ahci_port_priv *pp = ap->private_data;
-
-	/* IPM bits should be disabled by libata-core */
-	/* get the existing command bits */
-	cmd = readl(port_mmio + PORT_CMD);
-
-	/* disable ALPM and ASP */
-	cmd &= ~PORT_CMD_ASP;
-	cmd &= ~PORT_CMD_ALPE;
-
-	/* force the interface back to active */
-	cmd |= PORT_CMD_ICC_ACTIVE;
-
-	/* write out new cmd value */
-	writel(cmd, port_mmio + PORT_CMD);
-	cmd = readl(port_mmio + PORT_CMD);
-
-	/* wait 10ms to be sure we've come out of any low power state */
-	msleep(10);
-
-	/* clear out any PhyRdy stuff from interrupt status */
-	writel(PORT_IRQ_PHYRDY, port_mmio + PORT_IRQ_STAT);
-
-	/* go ahead and clean out PhyRdy Change from Serror too */
-	ahci_scr_write(&ap->link, SCR_ERROR, ((1 << 16) | (1 << 18)));
-
-	/*
- 	 * Clear flag to indicate that we should ignore all PhyRdy
- 	 * state changes
- 	 */
-	hpriv->flags &= ~AHCI_HFLAG_NO_HOTPLUG;
-
-	/*
- 	 * Enable interrupts on Phy Ready.
- 	 */
-	pp->intr_mask |= PORT_IRQ_PHYRDY;
-	writel(pp->intr_mask, port_mmio + PORT_IRQ_MASK);
-
-	/*
- 	 * don't change the link pm policy - we can be called
- 	 * just to turn of link pm temporarily
- 	 */
-}
-
-static int ahci_enable_alpm(struct ata_port *ap,
-	enum link_pm policy)
-{
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u32 cmd;
-	struct ahci_port_priv *pp = ap->private_data;
-	u32 asp;
-
-	/* Make sure the host is capable of link power management */
-	if (!(hpriv->cap & HOST_CAP_ALPM))
-		return -EINVAL;
-
-	switch (policy) {
-	case MAX_PERFORMANCE:
-	case NOT_AVAILABLE:
-		/*
- 		 * if we came here with NOT_AVAILABLE,
- 		 * it just means this is the first time we
- 		 * have tried to enable - default to max performance,
- 		 * and let the user go to lower power modes on request.
- 		 */
-		ahci_disable_alpm(ap);
-		return 0;
-	case MIN_POWER:
-		/* configure HBA to enter SLUMBER */
-		asp = PORT_CMD_ASP;
-		break;
-	case MEDIUM_POWER:
-		/* configure HBA to enter PARTIAL */
-		asp = 0;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	/*
- 	 * Disable interrupts on Phy Ready. This keeps us from
- 	 * getting woken up due to spurious phy ready interrupts
-	 * TBD - Hot plug should be done via polling now, is
-	 * that even supported?
- 	 */
-	pp->intr_mask &= ~PORT_IRQ_PHYRDY;
-	writel(pp->intr_mask, port_mmio + PORT_IRQ_MASK);
-
-	/*
- 	 * Set a flag to indicate that we should ignore all PhyRdy
- 	 * state changes since these can happen now whenever we
- 	 * change link state
- 	 */
-	hpriv->flags |= AHCI_HFLAG_NO_HOTPLUG;
-
-	/* get the existing command bits */
-	cmd = readl(port_mmio + PORT_CMD);
-
-	/*
- 	 * Set ASP based on Policy
- 	 */
-	cmd |= asp;
-
-	/*
- 	 * Setting this bit will instruct the HBA to aggressively
- 	 * enter a lower power link state when it's appropriate and
- 	 * based on the value set above for ASP
- 	 */
-	cmd |= PORT_CMD_ALPE;
-
-	/* write out new cmd value */
-	writel(cmd, port_mmio + PORT_CMD);
-	cmd = readl(port_mmio + PORT_CMD);
-
-	/* IPM bits should be set by libata-core */
-	return 0;
-}
-
-#ifdef CONFIG_PM
-static void ahci_power_down(struct ata_port *ap)
-{
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u32 cmd, scontrol;
-
-	if (!(hpriv->cap & HOST_CAP_SSS))
-		return;
-
-	/* put device into listen mode, first set PxSCTL.DET to 0 */
-	scontrol = readl(port_mmio + PORT_SCR_CTL);
-	scontrol &= ~0xf;
-	writel(scontrol, port_mmio + PORT_SCR_CTL);
-
-	/* then set PxCMD.SUD to 0 */
-	cmd = readl(port_mmio + PORT_CMD) & ~PORT_CMD_ICC_MASK;
-	cmd &= ~PORT_CMD_SPIN_UP;
-	writel(cmd, port_mmio + PORT_CMD);
-}
-#endif
-
-static void ahci_start_port(struct ata_port *ap)
-{
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ata_link *link;
-	struct ahci_em_priv *emp;
-	ssize_t rc;
-	int i;
-
-	/* enable FIS reception */
-	ahci_start_fis_rx(ap);
-
-	/* enable DMA */
-	ahci_start_engine(ap);
-
-	/* turn on LEDs */
-	if (ap->flags & ATA_FLAG_EM) {
-		ata_for_each_link(link, ap, EDGE) {
-			emp = &pp->em_priv[link->pmp];
-
-			/* EM Transmit bit maybe busy during init */
-			for (i = 0; i < EM_MAX_RETRY; i++) {
-				rc = ahci_transmit_led_message(ap,
-							       emp->led_state,
-							       4);
-				if (rc == -EBUSY)
-					msleep(1);
-				else
-					break;
-			}
-		}
-	}
-
-	if (ap->flags & ATA_FLAG_SW_ACTIVITY)
-		ata_for_each_link(link, ap, EDGE)
-			ahci_init_sw_activity(link);
-
-}
-
-static int ahci_deinit_port(struct ata_port *ap, const char **emsg)
-{
-	int rc;
-
-	/* disable DMA */
-	rc = ahci_stop_engine(ap);
-	if (rc) {
-		*emsg = "failed to stop engine";
-		return rc;
-	}
-
-	/* disable FIS reception */
-	rc = ahci_stop_fis_rx(ap);
-	if (rc) {
-		*emsg = "failed stop FIS RX";
-		return rc;
 	}
 
-	return 0;
+	ahci_save_initial_config(&pdev->dev, hpriv, force_port_map,
+				 mask_port_map);
 }
 
-static int ahci_reset_controller(struct ata_host *host)
+static int ahci_pci_reset_controller(struct ata_host *host)
 {
 	struct pci_dev *pdev = to_pci_dev(host->dev);
-	struct ahci_host_priv *hpriv = host->private_data;
-	void __iomem *mmio = host->iomap[AHCI_PCI_BAR];
-	u32 tmp;
 
-	/* we must be in AHCI mode, before using anything
-	 * AHCI-specific, such as HOST_RESET.
-	 */
-	ahci_enable_ahci(mmio);
-
-	/* global controller reset */
-	if (!ahci_skip_host_reset) {
-		tmp = readl(mmio + HOST_CTL);
-		if ((tmp & HOST_RESET) == 0) {
-			writel(tmp | HOST_RESET, mmio + HOST_CTL);
-			readl(mmio + HOST_CTL); /* flush */
-		}
-
-		/*
-		 * to perform host reset, OS should set HOST_RESET
-		 * and poll until this bit is read to be "0".
-		 * reset must complete within 1 second, or
-		 * the hardware should be considered fried.
-		 */
-		tmp = ata_wait_register(mmio + HOST_CTL, HOST_RESET,
-					HOST_RESET, 10, 1000);
-
-		if (tmp & HOST_RESET) {
-			dev_printk(KERN_ERR, host->dev,
-				   "controller reset failed (0x%x)\n", tmp);
-			return -EIO;
-		}
-
-		/* turn on AHCI mode */
-		ahci_enable_ahci(mmio);
-
-		/* Some registers might be cleared on reset.  Restore
-		 * initial values.
-		 */
-		ahci_restore_initial_config(host);
-	} else
-		dev_printk(KERN_INFO, host->dev,
-			   "skipping global host reset\n");
+	ahci_reset_controller(host);
 
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL) {
+		struct ahci_host_priv *hpriv = host->private_data;
 		u16 tmp16;
 
 		/* configure PCS */
@@ -1247,267 +400,10 @@ static int ahci_reset_controller(struct ata_host *host)
 	return 0;
 }
 
-static void ahci_sw_activity(struct ata_link *link)
-{
-	struct ata_port *ap = link->ap;
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ahci_em_priv *emp = &pp->em_priv[link->pmp];
-
-	if (!(link->flags & ATA_LFLAG_SW_ACTIVITY))
-		return;
-
-	emp->activity++;
-	if (!timer_pending(&emp->timer))
-		mod_timer(&emp->timer, jiffies + msecs_to_jiffies(10));
-}
-
-static void ahci_sw_activity_blink(unsigned long arg)
-{
-	struct ata_link *link = (struct ata_link *)arg;
-	struct ata_port *ap = link->ap;
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ahci_em_priv *emp = &pp->em_priv[link->pmp];
-	unsigned long led_message = emp->led_state;
-	u32 activity_led_state;
-	unsigned long flags;
-
-	led_message &= EM_MSG_LED_VALUE;
-	led_message |= ap->port_no | (link->pmp << 8);
-
-	/* check to see if we've had activity.  If so,
-	 * toggle state of LED and reset timer.  If not,
-	 * turn LED to desired idle state.
-	 */
-	spin_lock_irqsave(ap->lock, flags);
-	if (emp->saved_activity != emp->activity) {
-		emp->saved_activity = emp->activity;
-		/* get the current LED state */
-		activity_led_state = led_message & EM_MSG_LED_VALUE_ON;
-
-		if (activity_led_state)
-			activity_led_state = 0;
-		else
-			activity_led_state = 1;
-
-		/* clear old state */
-		led_message &= ~EM_MSG_LED_VALUE_ACTIVITY;
-
-		/* toggle state */
-		led_message |= (activity_led_state << 16);
-		mod_timer(&emp->timer, jiffies + msecs_to_jiffies(100));
-	} else {
-		/* switch to idle */
-		led_message &= ~EM_MSG_LED_VALUE_ACTIVITY;
-		if (emp->blink_policy == BLINK_OFF)
-			led_message |= (1 << 16);
-	}
-	spin_unlock_irqrestore(ap->lock, flags);
-	ahci_transmit_led_message(ap, led_message, 4);
-}
-
-static void ahci_init_sw_activity(struct ata_link *link)
-{
-	struct ata_port *ap = link->ap;
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ahci_em_priv *emp = &pp->em_priv[link->pmp];
-
-	/* init activity stats, setup timer */
-	emp->saved_activity = emp->activity = 0;
-	setup_timer(&emp->timer, ahci_sw_activity_blink, (unsigned long)link);
-
-	/* check our blink policy and set flag for link if it's enabled */
-	if (emp->blink_policy)
-		link->flags |= ATA_LFLAG_SW_ACTIVITY;
-}
-
-static int ahci_reset_em(struct ata_host *host)
-{
-	void __iomem *mmio = host->iomap[AHCI_PCI_BAR];
-	u32 em_ctl;
-
-	em_ctl = readl(mmio + HOST_EM_CTL);
-	if ((em_ctl & EM_CTL_TM) || (em_ctl & EM_CTL_RST))
-		return -EINVAL;
-
-	writel(em_ctl | EM_CTL_RST, mmio + HOST_EM_CTL);
-	return 0;
-}
-
-static ssize_t ahci_transmit_led_message(struct ata_port *ap, u32 state,
-					ssize_t size)
-{
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	struct ahci_port_priv *pp = ap->private_data;
-	void __iomem *mmio = ap->host->iomap[AHCI_PCI_BAR];
-	u32 em_ctl;
-	u32 message[] = {0, 0};
-	unsigned long flags;
-	int pmp;
-	struct ahci_em_priv *emp;
-
-	/* get the slot number from the message */
-	pmp = (state & EM_MSG_LED_PMP_SLOT) >> 8;
-	if (pmp < EM_MAX_SLOTS)
-		emp = &pp->em_priv[pmp];
-	else
-		return -EINVAL;
-
-	spin_lock_irqsave(ap->lock, flags);
-
-	/*
-	 * if we are still busy transmitting a previous message,
-	 * do not allow
-	 */
-	em_ctl = readl(mmio + HOST_EM_CTL);
-	if (em_ctl & EM_CTL_TM) {
-		spin_unlock_irqrestore(ap->lock, flags);
-		return -EBUSY;
-	}
-
-	/*
-	 * create message header - this is all zero except for
-	 * the message size, which is 4 bytes.
-	 */
-	message[0] |= (4 << 8);
-
-	/* ignore 0:4 of byte zero, fill in port info yourself */
-	message[1] = ((state & ~EM_MSG_LED_HBA_PORT) | ap->port_no);
-
-	/* write message to EM_LOC */
-	writel(message[0], mmio + hpriv->em_loc);
-	writel(message[1], mmio + hpriv->em_loc+4);
-
-	/* save off new led state for port/slot */
-	emp->led_state = state;
-
-	/*
-	 * tell hardware to transmit the message
-	 */
-	writel(em_ctl | EM_CTL_TM, mmio + HOST_EM_CTL);
-
-	spin_unlock_irqrestore(ap->lock, flags);
-	return size;
-}
-
-static ssize_t ahci_led_show(struct ata_port *ap, char *buf)
-{
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ata_link *link;
-	struct ahci_em_priv *emp;
-	int rc = 0;
-
-	ata_for_each_link(link, ap, EDGE) {
-		emp = &pp->em_priv[link->pmp];
-		rc += sprintf(buf, "%lx\n", emp->led_state);
-	}
-	return rc;
-}
-
-static ssize_t ahci_led_store(struct ata_port *ap, const char *buf,
-				size_t size)
-{
-	int state;
-	int pmp;
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ahci_em_priv *emp;
-
-	state = simple_strtoul(buf, NULL, 0);
-
-	/* get the slot number from the message */
-	pmp = (state & EM_MSG_LED_PMP_SLOT) >> 8;
-	if (pmp < EM_MAX_SLOTS)
-		emp = &pp->em_priv[pmp];
-	else
-		return -EINVAL;
-
-	/* mask off the activity bits if we are in sw_activity
-	 * mode, user should turn off sw_activity before setting
-	 * activity led through em_message
-	 */
-	if (emp->blink_policy)
-		state &= ~EM_MSG_LED_VALUE_ACTIVITY;
-
-	return ahci_transmit_led_message(ap, state, size);
-}
-
-static ssize_t ahci_activity_store(struct ata_device *dev, enum sw_activity val)
-{
-	struct ata_link *link = dev->link;
-	struct ata_port *ap = link->ap;
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ahci_em_priv *emp = &pp->em_priv[link->pmp];
-	u32 port_led_state = emp->led_state;
-
-	/* save the desired Activity LED behavior */
-	if (val == OFF) {
-		/* clear LFLAG */
-		link->flags &= ~(ATA_LFLAG_SW_ACTIVITY);
-
-		/* set the LED to OFF */
-		port_led_state &= EM_MSG_LED_VALUE_OFF;
-		port_led_state |= (ap->port_no | (link->pmp << 8));
-		ahci_transmit_led_message(ap, port_led_state, 4);
-	} else {
-		link->flags |= ATA_LFLAG_SW_ACTIVITY;
-		if (val == BLINK_OFF) {
-			/* set LED to ON for idle */
-			port_led_state &= EM_MSG_LED_VALUE_OFF;
-			port_led_state |= (ap->port_no | (link->pmp << 8));
-			port_led_state |= EM_MSG_LED_VALUE_ON; /* check this */
-			ahci_transmit_led_message(ap, port_led_state, 4);
-		}
-	}
-	emp->blink_policy = val;
-	return 0;
-}
-
-static ssize_t ahci_activity_show(struct ata_device *dev, char *buf)
-{
-	struct ata_link *link = dev->link;
-	struct ata_port *ap = link->ap;
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ahci_em_priv *emp = &pp->em_priv[link->pmp];
-
-	/* display the saved value of activity behavior for this
-	 * disk.
-	 */
-	return sprintf(buf, "%d\n", emp->blink_policy);
-}
-
-static void ahci_port_init(struct pci_dev *pdev, struct ata_port *ap,
-			   int port_no, void __iomem *mmio,
-			   void __iomem *port_mmio)
-{
-	const char *emsg = NULL;
-	int rc;
-	u32 tmp;
-
-	/* make sure port is not active */
-	rc = ahci_deinit_port(ap, &emsg);
-	if (rc)
-		dev_printk(KERN_WARNING, &pdev->dev,
-			   "%s (%d)\n", emsg, rc);
-
-	/* clear SError */
-	tmp = readl(port_mmio + PORT_SCR_ERR);
-	VPRINTK("PORT_SCR_ERR 0x%x\n", tmp);
-	writel(tmp, port_mmio + PORT_SCR_ERR);
-
-	/* clear port IRQ */
-	tmp = readl(port_mmio + PORT_IRQ_STAT);
-	VPRINTK("PORT_IRQ_STAT 0x%x\n", tmp);
-	if (tmp)
-		writel(tmp, port_mmio + PORT_IRQ_STAT);
-
-	writel(1 << port_no, mmio + HOST_IRQ_STAT);
-}
-
-static void ahci_init_controller(struct ata_host *host)
+static void ahci_pci_init_controller(struct ata_host *host)
 {
 	struct ahci_host_priv *hpriv = host->private_data;
 	struct pci_dev *pdev = to_pci_dev(host->dev);
-	void __iomem *mmio = host->iomap[AHCI_PCI_BAR];
-	int i;
 	void __iomem *port_mmio;
 	u32 tmp;
 	int mv;
@@ -1528,222 +424,7 @@ static void ahci_init_controller(struct ata_host *host)
 			writel(tmp, port_mmio + PORT_IRQ_STAT);
 	}
 
-	for (i = 0; i < host->n_ports; i++) {
-		struct ata_port *ap = host->ports[i];
-
-		port_mmio = ahci_port_base(ap);
-		if (ata_port_is_dummy(ap))
-			continue;
-
-		ahci_port_init(pdev, ap, i, mmio, port_mmio);
-	}
-
-	tmp = readl(mmio + HOST_CTL);
-	VPRINTK("HOST_CTL 0x%x\n", tmp);
-	writel(tmp | HOST_IRQ_EN, mmio + HOST_CTL);
-	tmp = readl(mmio + HOST_CTL);
-	VPRINTK("HOST_CTL 0x%x\n", tmp);
-}
-
-static void ahci_dev_config(struct ata_device *dev)
-{
-	struct ahci_host_priv *hpriv = dev->link->ap->host->private_data;
-
-	if (hpriv->flags & AHCI_HFLAG_SECT255) {
-		dev->max_sectors = 255;
-		ata_dev_printk(dev, KERN_INFO,
-			       "SB600 AHCI: limiting to 255 sectors per cmd\n");
-	}
-}
-
-static unsigned int ahci_dev_classify(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	struct ata_taskfile tf;
-	u32 tmp;
-
-	tmp = readl(port_mmio + PORT_SIG);
-	tf.lbah		= (tmp >> 24)	& 0xff;
-	tf.lbam		= (tmp >> 16)	& 0xff;
-	tf.lbal		= (tmp >> 8)	& 0xff;
-	tf.nsect	= (tmp)		& 0xff;
-
-	return ata_dev_classify(&tf);
-}
-
-static void ahci_fill_cmd_slot(struct ahci_port_priv *pp, unsigned int tag,
-			       u32 opts)
-{
-	dma_addr_t cmd_tbl_dma;
-
-	cmd_tbl_dma = pp->cmd_tbl_dma + tag * AHCI_CMD_TBL_SZ;
-
-	pp->cmd_slot[tag].opts = cpu_to_le32(opts);
-	pp->cmd_slot[tag].status = 0;
-	pp->cmd_slot[tag].tbl_addr = cpu_to_le32(cmd_tbl_dma & 0xffffffff);
-	pp->cmd_slot[tag].tbl_addr_hi = cpu_to_le32((cmd_tbl_dma >> 16) >> 16);
-}
-
-static int ahci_kick_engine(struct ata_port *ap, int force_restart)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	u8 status = readl(port_mmio + PORT_TFDATA) & 0xFF;
-	u32 tmp;
-	int busy, rc;
-
-	/* do we need to kick the port? */
-	busy = status & (ATA_BUSY | ATA_DRQ);
-	if (!busy && !force_restart)
-		return 0;
-
-	/* stop engine */
-	rc = ahci_stop_engine(ap);
-	if (rc)
-		goto out_restart;
-
-	/* need to do CLO? */
-	if (!busy) {
-		rc = 0;
-		goto out_restart;
-	}
-
-	if (!(hpriv->cap & HOST_CAP_CLO)) {
-		rc = -EOPNOTSUPP;
-		goto out_restart;
-	}
-
-	/* perform CLO */
-	tmp = readl(port_mmio + PORT_CMD);
-	tmp |= PORT_CMD_CLO;
-	writel(tmp, port_mmio + PORT_CMD);
-
-	rc = 0;
-	tmp = ata_wait_register(port_mmio + PORT_CMD,
-				PORT_CMD_CLO, PORT_CMD_CLO, 1, 500);
-	if (tmp & PORT_CMD_CLO)
-		rc = -EIO;
-
-	/* restart engine */
- out_restart:
-	ahci_start_engine(ap);
-	return rc;
-}
-
-static int ahci_exec_polled_cmd(struct ata_port *ap, int pmp,
-				struct ata_taskfile *tf, int is_cmd, u16 flags,
-				unsigned long timeout_msec)
-{
-	const u32 cmd_fis_len = 5; /* five dwords */
-	struct ahci_port_priv *pp = ap->private_data;
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u8 *fis = pp->cmd_tbl;
-	u32 tmp;
-
-	/* prep the command */
-	ata_tf_to_fis(tf, pmp, is_cmd, fis);
-	ahci_fill_cmd_slot(pp, 0, cmd_fis_len | flags | (pmp << 12));
-
-	/* issue & wait */
-	writel(1, port_mmio + PORT_CMD_ISSUE);
-
-	if (timeout_msec) {
-		tmp = ata_wait_register(port_mmio + PORT_CMD_ISSUE, 0x1, 0x1,
-					1, timeout_msec);
-		if (tmp & 0x1) {
-			ahci_kick_engine(ap, 1);
-			return -EBUSY;
-		}
-	} else
-		readl(port_mmio + PORT_CMD_ISSUE);	/* flush */
-
-	return 0;
-}
-
-static int ahci_do_softreset(struct ata_link *link, unsigned int *class,
-			     int pmp, unsigned long deadline,
-			     int (*check_ready)(struct ata_link *link))
-{
-	struct ata_port *ap = link->ap;
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	const char *reason = NULL;
-	unsigned long now, msecs;
-	struct ata_taskfile tf;
-	int rc;
-
-	DPRINTK("ENTER\n");
-
-	/* prepare for SRST (AHCI-1.1 10.4.1) */
-	rc = ahci_kick_engine(ap, 1);
-	if (rc && rc != -EOPNOTSUPP)
-		ata_link_printk(link, KERN_WARNING,
-				"failed to reset engine (errno=%d)\n", rc);
-
-	ata_tf_init(link->device, &tf);
-
-	/* issue the first D2H Register FIS */
-	msecs = 0;
-	now = jiffies;
-	if (time_after(now, deadline))
-		msecs = jiffies_to_msecs(deadline - now);
-
-	tf.ctl |= ATA_SRST;
-	if (ahci_exec_polled_cmd(ap, pmp, &tf, 0,
-				 AHCI_CMD_RESET | AHCI_CMD_CLR_BUSY, msecs)) {
-		rc = -EIO;
-		reason = "1st FIS failed";
-		goto fail;
-	}
-
-	/* spec says at least 5us, but be generous and sleep for 1ms */
-	msleep(1);
-
-	/* issue the second D2H Register FIS */
-	tf.ctl &= ~ATA_SRST;
-	ahci_exec_polled_cmd(ap, pmp, &tf, 0, 0, 0);
-
-	/* wait for link to become ready */
-	rc = ata_wait_after_reset(link, deadline, check_ready);
-	if (rc == -EBUSY && hpriv->flags & AHCI_HFLAG_SRST_TOUT_IS_OFFLINE) {
-		/*
-		 * Workaround for cases where link online status can't
-		 * be trusted.  Treat device readiness timeout as link
-		 * offline.
-		 */
-		ata_link_printk(link, KERN_INFO,
-				"device not ready, treating as offline\n");
-		*class = ATA_DEV_NONE;
-	} else if (rc) {
-		/* link occupied, -ENODEV too is an error */
-		reason = "device not ready";
-		goto fail;
-	} else
-		*class = ahci_dev_classify(ap);
-
-	DPRINTK("EXIT, class=%u\n", *class);
-	return 0;
-
- fail:
-	ata_link_printk(link, KERN_ERR, "softreset failed (%s)\n", reason);
-	return rc;
-}
-
-static int ahci_check_ready(struct ata_link *link)
-{
-	void __iomem *port_mmio = ahci_port_base(link->ap);
-	u8 status = readl(port_mmio + PORT_TFDATA) & 0xFF;
-
-	return ata_check_ready(status);
-}
-
-static int ahci_softreset(struct ata_link *link, unsigned int *class,
-			  unsigned long deadline)
-{
-	int pmp = sata_srst_pmp(link);
-
-	DPRINTK("ENTER\n");
-
-	return ahci_do_softreset(link, class, pmp, deadline, ahci_check_ready);
+	ahci_init_controller(host);
 }
 
 static int ahci_sb600_check_ready(struct ata_link *link)
@@ -1795,38 +476,6 @@ static int ahci_sb600_softreset(struct ata_link *link, unsigned int *class,
 	return rc;
 }
 
-static int ahci_hardreset(struct ata_link *link, unsigned int *class,
-			  unsigned long deadline)
-{
-	const unsigned long *timing = sata_ehc_deb_timing(&link->eh_context);
-	struct ata_port *ap = link->ap;
-	struct ahci_port_priv *pp = ap->private_data;
-	u8 *d2h_fis = pp->rx_fis + RX_FIS_D2H_REG;
-	struct ata_taskfile tf;
-	bool online;
-	int rc;
-
-	DPRINTK("ENTER\n");
-
-	ahci_stop_engine(ap);
-
-	/* clear D2H reception area to properly wait for D2H FIS */
-	ata_tf_init(link->device, &tf);
-	tf.command = 0x80;
-	ata_tf_to_fis(&tf, 0, 0, d2h_fis);
-
-	rc = sata_link_hardreset(link, timing, deadline, &online,
-				 ahci_check_ready);
-
-	ahci_start_engine(ap);
-
-	if (online)
-		*class = ahci_dev_classify(ap);
-
-	DPRINTK("EXIT, rc=%d, class=%u\n", rc, *class);
-	return rc;
-}
-
 static int ahci_vt8251_hardreset(struct ata_link *link, unsigned int *class,
 				 unsigned long deadline)
 {
@@ -1890,453 +539,17 @@ static int ahci_p5wdh_hardreset(struct ata_link *link, unsigned int *class,
 		rc = ata_wait_after_reset(link, jiffies + 2 * HZ,
 					  ahci_check_ready);
 		if (rc)
-			ahci_kick_engine(ap, 0);
+			ahci_kick_engine(ap);
 	}
 	return rc;
 }
 
-static void ahci_postreset(struct ata_link *link, unsigned int *class)
-{
-	struct ata_port *ap = link->ap;
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u32 new_tmp, tmp;
-
-	ata_std_postreset(link, class);
-
-	/* Make sure port's ATAPI bit is set appropriately */
-	new_tmp = tmp = readl(port_mmio + PORT_CMD);
-	if (*class == ATA_DEV_ATAPI)
-		new_tmp |= PORT_CMD_ATAPI;
-	else
-		new_tmp &= ~PORT_CMD_ATAPI;
-	if (new_tmp != tmp) {
-		writel(new_tmp, port_mmio + PORT_CMD);
-		readl(port_mmio + PORT_CMD); /* flush */
-	}
-}
-
-static unsigned int ahci_fill_sg(struct ata_queued_cmd *qc, void *cmd_tbl)
-{
-	struct scatterlist *sg;
-	struct ahci_sg *ahci_sg = cmd_tbl + AHCI_CMD_TBL_HDR_SZ;
-	unsigned int si;
-
-	VPRINTK("ENTER\n");
-
-	/*
-	 * Next, the S/G list.
-	 */
-	for_each_sg(qc->sg, sg, qc->n_elem, si) {
-		dma_addr_t addr = sg_dma_address(sg);
-		u32 sg_len = sg_dma_len(sg);
-
-		ahci_sg[si].addr = cpu_to_le32(addr & 0xffffffff);
-		ahci_sg[si].addr_hi = cpu_to_le32((addr >> 16) >> 16);
-		ahci_sg[si].flags_size = cpu_to_le32(sg_len - 1);
-	}
-
-	return si;
-}
-
-static void ahci_qc_prep(struct ata_queued_cmd *qc)
-{
-	struct ata_port *ap = qc->ap;
-	struct ahci_port_priv *pp = ap->private_data;
-	int is_atapi = ata_is_atapi(qc->tf.protocol);
-	void *cmd_tbl;
-	u32 opts;
-	const u32 cmd_fis_len = 5; /* five dwords */
-	unsigned int n_elem;
-
-	/*
-	 * Fill in command table information.  First, the header,
-	 * a SATA Register - Host to Device command FIS.
-	 */
-	cmd_tbl = pp->cmd_tbl + qc->tag * AHCI_CMD_TBL_SZ;
-
-	ata_tf_to_fis(&qc->tf, qc->dev->link->pmp, 1, cmd_tbl);
-	if (is_atapi) {
-		memset(cmd_tbl + AHCI_CMD_TBL_CDB, 0, 32);
-		memcpy(cmd_tbl + AHCI_CMD_TBL_CDB, qc->cdb, qc->dev->cdb_len);
-	}
-
-	n_elem = 0;
-	if (qc->flags & ATA_QCFLAG_DMAMAP)
-		n_elem = ahci_fill_sg(qc, cmd_tbl);
-
-	/*
-	 * Fill in command slot information.
-	 */
-	opts = cmd_fis_len | n_elem << 16 | (qc->dev->link->pmp << 12);
-	if (qc->tf.flags & ATA_TFLAG_WRITE)
-		opts |= AHCI_CMD_WRITE;
-	if (is_atapi)
-		opts |= AHCI_CMD_ATAPI | AHCI_CMD_PREFETCH;
-
-	ahci_fill_cmd_slot(pp, qc->tag, opts);
-}
-
-static void ahci_error_intr(struct ata_port *ap, u32 irq_stat)
-{
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ata_eh_info *host_ehi = &ap->link.eh_info;
-	struct ata_link *link = NULL;
-	struct ata_queued_cmd *active_qc;
-	struct ata_eh_info *active_ehi;
-	u32 serror;
-
-	/* determine active link */
-	ata_for_each_link(link, ap, EDGE)
-		if (ata_link_active(link))
-			break;
-	if (!link)
-		link = &ap->link;
-
-	active_qc = ata_qc_from_tag(ap, link->active_tag);
-	active_ehi = &link->eh_info;
-
-	/* record irq stat */
-	ata_ehi_clear_desc(host_ehi);
-	ata_ehi_push_desc(host_ehi, "irq_stat 0x%08x", irq_stat);
-
-	/* AHCI needs SError cleared; otherwise, it might lock up */
-	ahci_scr_read(&ap->link, SCR_ERROR, &serror);
-	ahci_scr_write(&ap->link, SCR_ERROR, serror);
-	host_ehi->serror |= serror;
-
-	/* some controllers set IRQ_IF_ERR on device errors, ignore it */
-	if (hpriv->flags & AHCI_HFLAG_IGN_IRQ_IF_ERR)
-		irq_stat &= ~PORT_IRQ_IF_ERR;
-
-	if (irq_stat & PORT_IRQ_TF_ERR) {
-		/* If qc is active, charge it; otherwise, the active
-		 * link.  There's no active qc on NCQ errors.  It will
-		 * be determined by EH by reading log page 10h.
-		 */
-		if (active_qc)
-			active_qc->err_mask |= AC_ERR_DEV;
-		else
-			active_ehi->err_mask |= AC_ERR_DEV;
-
-		if (hpriv->flags & AHCI_HFLAG_IGN_SERR_INTERNAL)
-			host_ehi->serror &= ~SERR_INTERNAL;
-	}
-
-	if (irq_stat & PORT_IRQ_UNK_FIS) {
-		u32 *unk = (u32 *)(pp->rx_fis + RX_FIS_UNK);
-
-		active_ehi->err_mask |= AC_ERR_HSM;
-		active_ehi->action |= ATA_EH_RESET;
-		ata_ehi_push_desc(active_ehi,
-				  "unknown FIS %08x %08x %08x %08x" ,
-				  unk[0], unk[1], unk[2], unk[3]);
-	}
-
-	if (sata_pmp_attached(ap) && (irq_stat & PORT_IRQ_BAD_PMP)) {
-		active_ehi->err_mask |= AC_ERR_HSM;
-		active_ehi->action |= ATA_EH_RESET;
-		ata_ehi_push_desc(active_ehi, "incorrect PMP");
-	}
-
-	if (irq_stat & (PORT_IRQ_HBUS_ERR | PORT_IRQ_HBUS_DATA_ERR)) {
-		host_ehi->err_mask |= AC_ERR_HOST_BUS;
-		host_ehi->action |= ATA_EH_RESET;
-		ata_ehi_push_desc(host_ehi, "host bus error");
-	}
-
-	if (irq_stat & PORT_IRQ_IF_ERR) {
-		host_ehi->err_mask |= AC_ERR_ATA_BUS;
-		host_ehi->action |= ATA_EH_RESET;
-		ata_ehi_push_desc(host_ehi, "interface fatal error");
-	}
-
-	if (irq_stat & (PORT_IRQ_CONNECT | PORT_IRQ_PHYRDY)) {
-		ata_ehi_hotplugged(host_ehi);
-		ata_ehi_push_desc(host_ehi, "%s",
-			irq_stat & PORT_IRQ_CONNECT ?
-			"connection status changed" : "PHY RDY changed");
-	}
-
-	/* okay, let's hand over to EH */
-
-	if (irq_stat & PORT_IRQ_FREEZE)
-		ata_port_freeze(ap);
-	else
-		ata_port_abort(ap);
-}
-
-static void ahci_port_intr(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	struct ata_eh_info *ehi = &ap->link.eh_info;
-	struct ahci_port_priv *pp = ap->private_data;
-	struct ahci_host_priv *hpriv = ap->host->private_data;
-	int resetting = !!(ap->pflags & ATA_PFLAG_RESETTING);
-	u32 status, qc_active;
-	int rc;
-
-	status = readl(port_mmio + PORT_IRQ_STAT);
-	writel(status, port_mmio + PORT_IRQ_STAT);
-
-	/* ignore BAD_PMP while resetting */
-	if (unlikely(resetting))
-		status &= ~PORT_IRQ_BAD_PMP;
-
-	/* If we are getting PhyRdy, this is
- 	 * just a power state change, we should
- 	 * clear out this, plus the PhyRdy/Comm
- 	 * Wake bits from Serror
- 	 */
-	if ((hpriv->flags & AHCI_HFLAG_NO_HOTPLUG) &&
-		(status & PORT_IRQ_PHYRDY)) {
-		status &= ~PORT_IRQ_PHYRDY;
-		ahci_scr_write(&ap->link, SCR_ERROR, ((1 << 16) | (1 << 18)));
-	}
-
-	if (unlikely(status & PORT_IRQ_ERROR)) {
-		ahci_error_intr(ap, status);
-		return;
-	}
-
-	if (status & PORT_IRQ_SDB_FIS) {
-		/* If SNotification is available, leave notification
-		 * handling to sata_async_notification().  If not,
-		 * emulate it by snooping SDB FIS RX area.
-		 *
-		 * Snooping FIS RX area is probably cheaper than
-		 * poking SNotification but some constrollers which
-		 * implement SNotification, ICH9 for example, don't
-		 * store AN SDB FIS into receive area.
-		 */
-		if (hpriv->cap & HOST_CAP_SNTF)
-			sata_async_notification(ap);
-		else {
-			/* If the 'N' bit in word 0 of the FIS is set,
-			 * we just received asynchronous notification.
-			 * Tell libata about it.
-			 */
-			const __le32 *f = pp->rx_fis + RX_FIS_SDB;
-			u32 f0 = le32_to_cpu(f[0]);
-
-			if (f0 & (1 << 15))
-				sata_async_notification(ap);
-		}
-	}
-
-	/* pp->active_link is valid iff any command is in flight */
-	if (ap->qc_active && pp->active_link->sactive)
-		qc_active = readl(port_mmio + PORT_SCR_ACT);
-	else
-		qc_active = readl(port_mmio + PORT_CMD_ISSUE);
-
-	rc = ata_qc_complete_multiple(ap, qc_active);
-
-	/* while resetting, invalid completions are expected */
-	if (unlikely(rc < 0 && !resetting)) {
-		ehi->err_mask |= AC_ERR_HSM;
-		ehi->action |= ATA_EH_RESET;
-		ata_port_freeze(ap);
-	}
-}
-
-static irqreturn_t ahci_interrupt(int irq, void *dev_instance)
-{
-	struct ata_host *host = dev_instance;
-	struct ahci_host_priv *hpriv;
-	unsigned int i, handled = 0;
-	void __iomem *mmio;
-	u32 irq_stat, irq_masked;
-
-	VPRINTK("ENTER\n");
-
-	hpriv = host->private_data;
-	mmio = host->iomap[AHCI_PCI_BAR];
-
-	/* sigh.  0xffffffff is a valid return from h/w */
-	irq_stat = readl(mmio + HOST_IRQ_STAT);
-	if (!irq_stat)
-		return IRQ_NONE;
-
-	irq_masked = irq_stat & hpriv->port_map;
-
-	spin_lock(&host->lock);
-
-	for (i = 0; i < host->n_ports; i++) {
-		struct ata_port *ap;
-
-		if (!(irq_masked & (1 << i)))
-			continue;
-
-		ap = host->ports[i];
-		if (ap) {
-			ahci_port_intr(ap);
-			VPRINTK("port %u\n", i);
-		} else {
-			VPRINTK("port %u (no irq)\n", i);
-			if (ata_ratelimit())
-				dev_printk(KERN_WARNING, host->dev,
-					"interrupt on disabled port %u\n", i);
-		}
-
-		handled = 1;
-	}
-
-	/* HOST_IRQ_STAT behaves as level triggered latch meaning that
-	 * it should be cleared after all the port events are cleared;
-	 * otherwise, it will raise a spurious interrupt after each
-	 * valid one.  Please read section 10.6.2 of ahci 1.1 for more
-	 * information.
-	 *
-	 * Also, use the unmasked value to clear interrupt as spurious
-	 * pending event on a dummy port might cause screaming IRQ.
-	 */
-	writel(irq_stat, mmio + HOST_IRQ_STAT);
-
-	spin_unlock(&host->lock);
-
-	VPRINTK("EXIT\n");
-
-	return IRQ_RETVAL(handled);
-}
-
-static unsigned int ahci_qc_issue(struct ata_queued_cmd *qc)
-{
-	struct ata_port *ap = qc->ap;
-	void __iomem *port_mmio = ahci_port_base(ap);
-	struct ahci_port_priv *pp = ap->private_data;
-
-	/* Keep track of the currently active link.  It will be used
-	 * in completion path to determine whether NCQ phase is in
-	 * progress.
-	 */
-	pp->active_link = qc->dev->link;
-
-	if (qc->tf.protocol == ATA_PROT_NCQ)
-		writel(1 << qc->tag, port_mmio + PORT_SCR_ACT);
-	writel(1 << qc->tag, port_mmio + PORT_CMD_ISSUE);
-
-	ahci_sw_activity(qc->dev->link);
-
-	return 0;
-}
-
-static bool ahci_qc_fill_rtf(struct ata_queued_cmd *qc)
-{
-	struct ahci_port_priv *pp = qc->ap->private_data;
-	u8 *d2h_fis = pp->rx_fis + RX_FIS_D2H_REG;
-
-	ata_tf_from_fis(d2h_fis, &qc->result_tf);
-	return true;
-}
-
-static void ahci_freeze(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-
-	/* turn IRQ off */
-	writel(0, port_mmio + PORT_IRQ_MASK);
-}
-
-static void ahci_thaw(struct ata_port *ap)
-{
-	void __iomem *mmio = ap->host->iomap[AHCI_PCI_BAR];
-	void __iomem *port_mmio = ahci_port_base(ap);
-	u32 tmp;
-	struct ahci_port_priv *pp = ap->private_data;
-
-	/* clear IRQ */
-	tmp = readl(port_mmio + PORT_IRQ_STAT);
-	writel(tmp, port_mmio + PORT_IRQ_STAT);
-	writel(1 << ap->port_no, mmio + HOST_IRQ_STAT);
-
-	/* turn IRQ back on */
-	writel(pp->intr_mask, port_mmio + PORT_IRQ_MASK);
-}
-
-static void ahci_error_handler(struct ata_port *ap)
-{
-	if (!(ap->pflags & ATA_PFLAG_FROZEN)) {
-		/* restart engine */
-		ahci_stop_engine(ap);
-		ahci_start_engine(ap);
-	}
-
-	sata_pmp_error_handler(ap);
-}
-
-static void ahci_post_internal_cmd(struct ata_queued_cmd *qc)
-{
-	struct ata_port *ap = qc->ap;
-
-	/* make DMA engine forget about the failed command */
-	if (qc->flags & ATA_QCFLAG_FAILED)
-		ahci_kick_engine(ap, 1);
-}
-
-static void ahci_pmp_attach(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	struct ahci_port_priv *pp = ap->private_data;
-	u32 cmd;
-
-	cmd = readl(port_mmio + PORT_CMD);
-	cmd |= PORT_CMD_PMP;
-	writel(cmd, port_mmio + PORT_CMD);
-
-	pp->intr_mask |= PORT_IRQ_BAD_PMP;
-	writel(pp->intr_mask, port_mmio + PORT_IRQ_MASK);
-}
-
-static void ahci_pmp_detach(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	struct ahci_port_priv *pp = ap->private_data;
-	u32 cmd;
-
-	cmd = readl(port_mmio + PORT_CMD);
-	cmd &= ~PORT_CMD_PMP;
-	writel(cmd, port_mmio + PORT_CMD);
-
-	pp->intr_mask &= ~PORT_IRQ_BAD_PMP;
-	writel(pp->intr_mask, port_mmio + PORT_IRQ_MASK);
-}
-
-static int ahci_port_resume(struct ata_port *ap)
-{
-	ahci_power_up(ap);
-	ahci_start_port(ap);
-
-	if (sata_pmp_attached(ap))
-		ahci_pmp_attach(ap);
-	else
-		ahci_pmp_detach(ap);
-
-	return 0;
-}
-
 #ifdef CONFIG_PM
-static int ahci_port_suspend(struct ata_port *ap, pm_message_t mesg)
-{
-	const char *emsg = NULL;
-	int rc;
-
-	rc = ahci_deinit_port(ap, &emsg);
-	if (rc == 0)
-		ahci_power_down(ap);
-	else {
-		ata_port_printk(ap, KERN_ERR, "%s (%d)\n", emsg, rc);
-		ahci_start_port(ap);
-	}
-
-	return rc;
-}
-
 static int ahci_pci_device_suspend(struct pci_dev *pdev, pm_message_t mesg)
 {
 	struct ata_host *host = dev_get_drvdata(&pdev->dev);
 	struct ahci_host_priv *hpriv = host->private_data;
-	void __iomem *mmio = host->iomap[AHCI_PCI_BAR];
+	void __iomem *mmio = hpriv->mmio;
 	u32 ctl;
 
 	if (mesg.event & PM_EVENT_SUSPEND &&
@@ -2370,11 +583,11 @@ static int ahci_pci_device_resume(struct pci_dev *pdev)
 		return rc;
 
 	if (pdev->dev.power.power_state.event == PM_EVENT_SUSPEND) {
-		rc = ahci_reset_controller(host);
+		rc = ahci_pci_reset_controller(host);
 		if (rc)
 			return rc;
 
-		ahci_init_controller(host);
+		ahci_pci_init_controller(host);
 	}
 
 	ata_host_resume(host);
@@ -2383,72 +596,6 @@ static int ahci_pci_device_resume(struct pci_dev *pdev)
 }
 #endif
 
-static int ahci_port_start(struct ata_port *ap)
-{
-	struct device *dev = ap->host->dev;
-	struct ahci_port_priv *pp;
-	void *mem;
-	dma_addr_t mem_dma;
-
-	pp = devm_kzalloc(dev, sizeof(*pp), GFP_KERNEL);
-	if (!pp)
-		return -ENOMEM;
-
-	mem = dmam_alloc_coherent(dev, AHCI_PORT_PRIV_DMA_SZ, &mem_dma,
-				  GFP_KERNEL);
-	if (!mem)
-		return -ENOMEM;
-	memset(mem, 0, AHCI_PORT_PRIV_DMA_SZ);
-
-	/*
-	 * First item in chunk of DMA memory: 32-slot command table,
-	 * 32 bytes each in size
-	 */
-	pp->cmd_slot = mem;
-	pp->cmd_slot_dma = mem_dma;
-
-	mem += AHCI_CMD_SLOT_SZ;
-	mem_dma += AHCI_CMD_SLOT_SZ;
-
-	/*
-	 * Second item: Received-FIS area
-	 */
-	pp->rx_fis = mem;
-	pp->rx_fis_dma = mem_dma;
-
-	mem += AHCI_RX_FIS_SZ;
-	mem_dma += AHCI_RX_FIS_SZ;
-
-	/*
-	 * Third item: data area for storing a single command
-	 * and its scatter-gather table
-	 */
-	pp->cmd_tbl = mem;
-	pp->cmd_tbl_dma = mem_dma;
-
-	/*
-	 * Save off initial list of interrupts to be enabled.
-	 * This could be changed later
-	 */
-	pp->intr_mask = DEF_PORT_IRQ;
-
-	ap->private_data = pp;
-
-	/* engage engines, captain */
-	return ahci_port_resume(ap);
-}
-
-static void ahci_port_stop(struct ata_port *ap)
-{
-	const char *emsg = NULL;
-	int rc;
-
-	/* de-initialize port */
-	rc = ahci_deinit_port(ap, &emsg);
-	if (rc)
-		ata_port_printk(ap, KERN_WARNING, "%s (%d)\n", emsg, rc);
-}
-
 static int ahci_configure_dma_masks(struct pci_dev *pdev, int using_dac)
 {
 	int rc;
@@ -2481,30 +628,12 @@ static int ahci_configure_dma_masks(struct pci_dev *pdev, int using_dac)
 	return 0;
 }
 
-static void ahci_print_info(struct ata_host *host)
+static void ahci_pci_print_info(struct ata_host *host)
 {
-	struct ahci_host_priv *hpriv = host->private_data;
 	struct pci_dev *pdev = to_pci_dev(host->dev);
-	void __iomem *mmio = host->iomap[AHCI_PCI_BAR];
-	u32 vers, cap, impl, speed;
-	const char *speed_s;
 	u16 cc;
 	const char *scc_s;
 
-	vers = readl(mmio + HOST_VERSION);
-	cap = hpriv->cap;
-	impl = hpriv->port_map;
-
-	speed = (cap >> 20) & 0xf;
-	if (speed == 1)
-		speed_s = "1.5";
-	else if (speed == 2)
-		speed_s = "3";
-	else if (speed == 3)
-		speed_s = "6";
-	else
-		speed_s = "?";
-
 	pci_read_config_word(pdev, 0x0a, &cc);
 	if (cc == PCI_CLASS_STORAGE_IDE)
 		scc_s = "IDE";
@@ -2515,46 +644,7 @@ static void ahci_print_info(struct ata_host *host)
 	else
 		scc_s = "unknown";
 
-	dev_printk(KERN_INFO, &pdev->dev,
-		"AHCI %02x%02x.%02x%02x "
-		"%u slots %u ports %s Gbps 0x%x impl %s mode\n"
-		,
-
-		(vers >> 24) & 0xff,
-		(vers >> 16) & 0xff,
-		(vers >> 8) & 0xff,
-		vers & 0xff,
-
-		((cap >> 8) & 0x1f) + 1,
-		(cap & 0x1f) + 1,
-		speed_s,
-		impl,
-		scc_s);
-
-	dev_printk(KERN_INFO, &pdev->dev,
-		"flags: "
-		"%s%s%s%s%s%s%s"
-		"%s%s%s%s%s%s%s"
-		"%s\n"
-		,
-
-		cap & (1 << 31) ? "64bit " : "",
-		cap & (1 << 30) ? "ncq " : "",
-		cap & (1 << 29) ? "sntf " : "",
-		cap & (1 << 28) ? "ilck " : "",
-		cap & (1 << 27) ? "stag " : "",
-		cap & (1 << 26) ? "pm " : "",
-		cap & (1 << 25) ? "led " : "",
-
-		cap & (1 << 24) ? "clo " : "",
-		cap & (1 << 19) ? "nz " : "",
-		cap & (1 << 18) ? "only " : "",
-		cap & (1 << 17) ? "pmp " : "",
-		cap & (1 << 15) ? "pio " : "",
-		cap & (1 << 14) ? "slum " : "",
-		cap & (1 << 13) ? "part " : "",
-		cap & (1 << 6) ? "ems ": ""
-		);
+	ahci_print_info(host, scc_s);
 }
 
 /* On ASUS P5W DH Deluxe, the second port of PCI device 00:1f.2 is
@@ -2789,6 +879,55 @@ static bool ahci_broken_online(struct pci_dev *pdev)
 	return pdev->bus->number == (val >> 8) && pdev->devfn == (val & 0xff);
 }
 
+#ifdef CONFIG_ATA_ACPI
+static void ahci_gtf_filter_workaround(struct ata_host *host)
+{
+	static const struct dmi_system_id sysids[] = {
+		/*
+		 * Aspire 3810T issues a bunch of SATA enable commands
+		 * via _GTF including an invalid one and one which is
+		 * rejected by the device.  Among the successful ones
+		 * is FPDMA non-zero offset enable which when enabled
+		 * only on the drive side leads to NCQ command
+		 * failures.  Filter it out.
+		 */
+		{
+			.ident = "Aspire 3810T",
+			.matches = {
+				DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
+				DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 3810T"),
+			},
+			.driver_data = (void *)ATA_ACPI_FILTER_FPDMA_OFFSET,
+		},
+		{ }
+	};
+	const struct dmi_system_id *dmi = dmi_first_match(sysids);
+	unsigned int filter;
+	int i;
+
+	if (!dmi)
+		return;
+
+	filter = (unsigned long)dmi->driver_data;
+	dev_printk(KERN_INFO, host->dev,
+		   "applying extra ACPI _GTF filter 0x%x for %s\n",
+		   filter, dmi->ident);
+
+	for (i = 0; i < host->n_ports; i++) {
+		struct ata_port *ap = host->ports[i];
+		struct ata_link *link;
+		struct ata_device *dev;
+
+		ata_for_each_link(link, ap, EDGE)
+			ata_for_each_dev(dev, link, ALL)
+				dev->gtf_filter |= filter;
+	}
+}
+#else
+static inline void ahci_gtf_filter_workaround(struct ata_host *host)
+{}
+#endif
+
 static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	static int printed_version;
@@ -2864,33 +1003,19 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (!(hpriv->flags & AHCI_HFLAG_NO_MSI))
 		pci_enable_msi(pdev);
 
+	hpriv->mmio = pcim_iomap_table(pdev)[AHCI_PCI_BAR];
+
 	/* save initial config */
-	ahci_save_initial_config(pdev, hpriv);
+	ahci_pci_save_initial_config(pdev, hpriv);
 
 	/* prepare host */
 	if (hpriv->cap & HOST_CAP_NCQ)
-		pi.flags |= ATA_FLAG_NCQ;
+		pi.flags |= ATA_FLAG_NCQ | ATA_FLAG_FPDMA_AA;
 
 	if (hpriv->cap & HOST_CAP_PMP)
 		pi.flags |= ATA_FLAG_PMP;
 
-	if (ahci_em_messages && (hpriv->cap & HOST_CAP_EMS)) {
-		u8 messages;
-		void __iomem *mmio = pcim_iomap_table(pdev)[AHCI_PCI_BAR];
-		u32 em_loc = readl(mmio + HOST_EM_LOC);
-		u32 em_ctl = readl(mmio + HOST_EM_CTL);
-
-		messages = (em_ctl & EM_CTRL_MSG_TYPE) >> 16;
-
-		/* we only support LED message type right now */
-		if ((messages & 0x01) && (ahci_em_messages == 1)) {
-			/* store em_loc */
-			hpriv->em_loc = ((em_loc >> 16) * 4);
-			pi.flags |= ATA_FLAG_EM;
-			if (!(em_ctl & EM_CTL_ALHD))
-				pi.flags |= ATA_FLAG_SW_ACTIVITY;
-		}
-	}
+	ahci_set_em_messages(hpriv, &pi);
 
 	if (ahci_broken_system_poweroff(pdev)) {
 		pi.flags |= ATA_FLAG_NO_POWEROFF_SPINDOWN;
@@ -2920,7 +1045,6 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	host = ata_host_alloc_pinfo(&pdev->dev, ppi, n_ports);
 	if (!host)
 		return -ENOMEM;
-	host->iomap = pcim_iomap_table(pdev);
 	host->private_data = hpriv;
 
 	if (!(hpriv->cap & HOST_CAP_SSS) || ahci_ignore_sss)
@@ -2954,17 +1078,20 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* apply workaround for ASUS P5W DH Deluxe mainboard */
 	ahci_p5wdh_workaround(host);
 
+	/* apply gtf filter quirk */
+	ahci_gtf_filter_workaround(host);
+
 	/* initialize adapter */
 	rc = ahci_configure_dma_masks(pdev, hpriv->cap & HOST_CAP_64);
 	if (rc)
 		return rc;
 
-	rc = ahci_reset_controller(host);
+	rc = ahci_pci_reset_controller(host);
 	if (rc)
 		return rc;
 
-	ahci_init_controller(host);
-	ahci_print_info(host);
+	ahci_pci_init_controller(host);
+	ahci_pci_print_info(host);
 
 	pci_set_master(pdev);
 	return ata_host_activate(host, pdev->irq, ahci_interrupt, IRQF_SHARED,
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index 072ba5ea138f..98af50f16e0c 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -2299,29 +2299,49 @@ static inline u8 ata_dev_knobble(struct ata_device *dev)
 	return ((ap->cbl == ATA_CBL_SATA) && (!ata_id_is_sata(dev->id)));
 }
 
-static void ata_dev_config_ncq(struct ata_device *dev,
+static int ata_dev_config_ncq(struct ata_device *dev,
 			       char *desc, size_t desc_sz)
 {
 	struct ata_port *ap = dev->link->ap;
 	int hdepth = 0, ddepth = ata_id_queue_depth(dev->id);
+	unsigned int err_mask;
+	char *aa_desc = "";
 
 	if (!ata_id_has_ncq(dev->id)) {
 		desc[0] = '\0';
-		return;
+		return 0;
 	}
 	if (dev->horkage & ATA_HORKAGE_NONCQ) {
 		snprintf(desc, desc_sz, "NCQ (not used)");
-		return;
+		return 0;
 	}
 	if (ap->flags & ATA_FLAG_NCQ) {
 		hdepth = min(ap->scsi_host->can_queue, ATA_MAX_QUEUE - 1);
 		dev->flags |= ATA_DFLAG_NCQ;
 	}
 
+	if (!(dev->horkage & ATA_HORKAGE_BROKEN_FPDMA_AA) &&
+		(ap->flags & ATA_FLAG_FPDMA_AA) &&
+		ata_id_has_fpdma_aa(dev->id)) {
+		err_mask = ata_dev_set_feature(dev, SETFEATURES_SATA_ENABLE,
+			SATA_FPDMA_AA);
+		if (err_mask) {
+			ata_dev_printk(dev, KERN_ERR, "failed to enable AA"
+				"(error_mask=0x%x)\n", err_mask);
+			if (err_mask != AC_ERR_DEV) {
+				dev->horkage |= ATA_HORKAGE_BROKEN_FPDMA_AA;
+				return -EIO;
+			}
+		} else
+			aa_desc = ", AA";
+	}
+
 	if (hdepth >= ddepth)
-		snprintf(desc, desc_sz, "NCQ (depth %d)", ddepth);
+		snprintf(desc, desc_sz, "NCQ (depth %d)%s", ddepth, aa_desc);
 	else
-		snprintf(desc, desc_sz, "NCQ (depth %d/%d)", hdepth, ddepth);
+		snprintf(desc, desc_sz, "NCQ (depth %d/%d)%s", hdepth,
+			ddepth, aa_desc);
+	return 0;
 }
 
 /**
@@ -2461,7 +2481,7 @@ int ata_dev_configure(struct ata_device *dev)
 
 		if (ata_id_has_lba(id)) {
 			const char *lba_desc;
-			char ncq_desc[20];
+			char ncq_desc[24];
 
 			lba_desc = "LBA";
 			dev->flags |= ATA_DFLAG_LBA;
@@ -2475,7 +2495,9 @@ int ata_dev_configure(struct ata_device *dev)
 			}
 
 			/* config NCQ */
-			ata_dev_config_ncq(dev, ncq_desc, sizeof(ncq_desc));
+			rc = ata_dev_config_ncq(dev, ncq_desc, sizeof(ncq_desc));
+			if (rc)
+				return rc;
 
 			/* print device info to dmesg */
 			if (ata_msg_drv(ap) && print_info) {
diff --git a/drivers/ata/pata_fsl.c b/drivers/ata/pata_fsl.c
index c1d05282da03..955095039257 100644
--- a/drivers/ata/pata_fsl.c
+++ b/drivers/ata/pata_fsl.c
@@ -3,7 +3,7 @@
  */
 
 /*
- * Copyright 2007-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2007-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -766,10 +766,11 @@ static int __devinit pata_fsl_probe(struct platform_device *pdev)
 	/*
 	 * Set up resources
 	 */
-	if (unlikely(pdev->num_resources != 3)) {
+	if (unlikely(pdev->num_resources != 2)) {
 		dev_err(&pdev->dev, "invalid number of resources\n");
 		return -EINVAL;
 	}
+
 	/*
 	 * Get an ata_host structure for this device
 	 */
diff --git a/drivers/ata/pata_pcmcia.c b/drivers/ata/pata_pcmcia.c
index dc99e26f8e5b..c4647f5b6a22 100644
--- a/drivers/ata/pata_pcmcia.c
+++ b/drivers/ata/pata_pcmcia.c
@@ -136,7 +136,7 @@ static unsigned int ata_data_xfer_8bit(struct ata_device *dev,
  *
  */
  
-void pcmcia_8bit_drain_fifo(struct ata_queued_cmd *qc)
+static void pcmcia_8bit_drain_fifo(struct ata_queued_cmd *qc)
 {
 	int count;
 	struct ata_port *ap;
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index 8efaa5ca8a03..49f086977c37 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -444,7 +444,7 @@ config MXC_IIM
 
 config MXS_VIIM
 	tristate "MXS Virtual IIM device driver"
-	depends on (ARCH_STMP3XXX || ARCH_MXS)
+	depends on (ARCH_STMP3XXX || ARCH_MXS || ARCH_MX5)
 	help
 	  Support for access to MXS Virtual IIM device, most people should say N here.
 
@@ -564,6 +564,22 @@ config BFIN_OTP_WRITE_ENABLE
 
 	  If unsure, say N.
 
+config FSL_OTP
+	tristate "Freescale On-Chip OTP Memory Support"
+	depends on (ARCH_MX23 || ARCH_MX28 || ARCH_MX50)
+	default n
+	help
+	  If you say Y here, you will get support for a character device
+	  interface into the One Time Programmable memory pages that are
+	  stored on the iMX23/28/50 processor.  This will not get you access
+	  to the secure memory pages however.  You will need to write your
+	  own secure code and reader for that.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called fsl_otp.
+
+	  If unsure, it is safe to say Y.
+
 config PRINTER
 	tristate "Parallel printer support"
 	depends on PARPORT
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index c711f02de8f7..898ce27e335d 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -69,6 +69,7 @@ obj-$(CONFIG_IBM_BSR)		+= bsr.o
 obj-$(CONFIG_SGI_MBCS)		+= mbcs.o
 obj-$(CONFIG_BRIQ_PANEL)	+= briq_panel.o
 obj-$(CONFIG_BFIN_OTP)		+= bfin-otp.o
+obj-$(CONFIG_FSL_OTP)		+= fsl_otp.o
 
 obj-$(CONFIG_PRINTER)		+= lp.o
 
diff --git a/drivers/char/hw_random/fsl-rngc.c b/drivers/char/hw_random/fsl-rngc.c
index 9bf78e846fa0..9e788f97c4d8 100644
--- a/drivers/char/hw_random/fsl-rngc.c
+++ b/drivers/char/hw_random/fsl-rngc.c
@@ -1,7 +1,7 @@
 /*
  * RNG driver for Freescale RNGC
  *
- * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2008-2010 Freescale Semiconductor, Inc.
  */
 
 /*
@@ -42,7 +42,6 @@
 #include <linux/interrupt.h>
 #include <linux/hw_random.h>
 #include <linux/io.h>
-#include <asm/hardware.h>
 
 #define RNGC_VERSION_MAJOR3 3
 
@@ -292,7 +291,7 @@ static int __init fsl_rngc_probe(struct platform_device *pdev)
 	if (rng_dev)
 		return -EBUSY;
 
-	clk = clk_get(NULL, "rng_clk");
+	clk = clk_get(&pdev->dev, "rng_clk");
 
 	if (IS_ERR(clk)) {
 		dev_err(&pdev->dev, "Can not get rng_clk\n");
@@ -334,9 +333,17 @@ static int __init fsl_rngc_probe(struct platform_device *pdev)
 
 static int __exit fsl_rngc_remove(struct platform_device *pdev)
 {
+	struct clk *clk;
 	struct resource *mem = dev_get_drvdata(&pdev->dev);
 	void __iomem *rngc_base = (void __iomem *)fsl_rngc.priv;
 
+	clk = clk_get(&pdev->dev, "rng_clk");
+
+	if (IS_ERR(clk))
+		dev_err(&pdev->dev, "Can not get rng_clk\n");
+	else
+		clk_disable(clk);
+
 	hwrng_unregister(&fsl_rngc);
 
 	release_resource(mem);
@@ -346,12 +353,47 @@ static int __exit fsl_rngc_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static int fsl_rngc_suspend(struct platform_device *pdev,
+		pm_message_t state)
+{
+#ifdef CONFIG_PM
+	struct clk *clk = clk_get(&pdev->dev, "rng_clk");
+
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "Can not get rng_clk\n");
+		return PTR_ERR(clk);
+	}
+
+	clk_disable(clk);
+#endif
+
+	return 0;
+}
+
+static int fsl_rngc_resume(struct platform_device *pdev)
+{
+#ifdef CONFIG_PM
+	struct clk *clk = clk_get(&pdev->dev, "rng_clk");
+
+	if (IS_ERR(clk)) {
+		dev_err(&pdev->dev, "Can not get rng_clk\n");
+		return PTR_ERR(clk);
+	}
+
+	clk_enable(clk);
+#endif
+
+	return 0;
+}
+
 static struct platform_driver fsl_rngc_driver = {
 	.driver = {
 		   .name = "fsl_rngc",
 		   .owner = THIS_MODULE,
 		   },
 	.remove = __exit_p(fsl_rngc_remove),
+	.suspend	= fsl_rngc_suspend,
+	.resume	= fsl_rngc_resume,
 };
 
 static int __init mod_init(void)
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index 9c2919a431a8..5057cb958230 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -211,7 +211,7 @@ config CRYPTO_DEV_PPC4XX
 
 config CRYPTO_DEV_DCP
 	tristate "Support for the DCP engine"
-	depends on ARCH_MX28 || ARCH_MX23
+	depends on ARCH_MX28 || ARCH_MX23 || ARCH_MX50
 	select CRYPTO_ALGAPI
 	select CRYPTO_BLKCIPHER
 	help
diff --git a/drivers/crypto/dcp.c b/drivers/crypto/dcp.c
index a72d73382778..eb7a83d276b7 100644
--- a/drivers/crypto/dcp.c
+++ b/drivers/crypto/dcp.c
@@ -17,10 +17,16 @@
 
 #include <linux/module.h>
 #include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/sysdev.h>
+#include <linux/bitops.h>
 #include <linux/crypto.h>
 #include <linux/spinlock.h>
+#include <linux/miscdevice.h>
 #include <linux/platform_device.h>
 #include <linux/err.h>
+#include <linux/sysfs.h>
+#include <linux/fs.h>
 #include <crypto/algapi.h>
 #include <crypto/aes.h>
 #include <crypto/sha.h>
@@ -29,13 +35,21 @@
 #include <linux/dma-mapping.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
-
+#include <linux/uaccess.h>
+#include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/delay.h>
 
 #include <asm/cacheflush.h>
 #include <mach/hardware.h>
 #include "dcp.h"
+#include "dcp_bootstream_ioctl.h"
+
+/* Following data only used by DCP bootstream interface */
+struct dcpboot_dma_area {
+	struct dcp_hw_packet hw_packet;
+	uint16_t block[16];
+};
 
 struct dcp {
 	struct device *dev;
@@ -46,6 +60,8 @@ struct dcp {
 	int dcp_vmi_irq;
 	int dcp_irq;
 	u32 dcp_regs_base;
+	ulong clock_state;
+	bool chan_in_use[DCP_NUM_CHANNELS];
 
 	/* Following buffers used in hashing to meet 64-byte len alignment */
 	char *buf1;
@@ -55,6 +71,10 @@ struct dcp {
 	struct dcp_hash_coherent_block *buf1_desc;
 	struct dcp_hash_coherent_block *buf2_desc;
 	struct dcp_hash_coherent_block *user_buf_desc;
+
+	/* Following data only used by DCP bootstream interface */
+	struct dcpboot_dma_area *dcpboot_dma_area;
+	dma_addr_t dcpboot_dma_area_phys;
 };
 
 /* cipher flags */
@@ -77,6 +97,10 @@ struct dcp {
 #define DCP_FILL	0x5000
 #define DCP_MODE_MASK	0xf000
 
+/* clock defines */
+#define CLOCK_ON	1
+#define CLOCK_OFF	0
+
 struct dcp_op {
 
 	unsigned int flags;
@@ -159,6 +183,45 @@ struct dcp_hash_op {
 /* only one */
 static struct dcp *global_sdcp;
 
+static void dcp_clock(struct dcp *sdcp,  ulong state, bool force)
+{
+	u32 chan;
+	struct clk *clk = clk_get(sdcp->dev, "dcp_clk");
+
+	/* unless force is true (used during suspend/resume), if any
+	  * channel is running, then clk is already on, and must stay on */
+	if (!force)
+		for (chan = 0; chan < DCP_NUM_CHANNELS; chan++)
+			if (sdcp->chan_in_use[chan])
+				goto exit;
+
+	if (state == CLOCK_OFF) {
+		/* gate at clock source */
+		if (!IS_ERR(clk))
+			clk_disable(clk);
+		/* gate at DCP */
+		else
+			__raw_writel(BM_DCP_CTRL_CLKGATE,
+				sdcp->dcp_regs_base + HW_DCP_CTRL_SET);
+
+		sdcp->clock_state = CLOCK_OFF;
+
+	} else {
+		/* ungate at clock source */
+		if (!IS_ERR(clk))
+			clk_enable(clk);
+		/* ungate at DCP */
+		else
+			__raw_writel(BM_DCP_CTRL_CLKGATE,
+				sdcp->dcp_regs_base + HW_DCP_CTRL_CLR);
+
+		sdcp->clock_state = CLOCK_ON;
+	}
+
+exit:
+	return;
+}
+
 static void dcp_perform_op(struct dcp_op *op)
 {
 	struct dcp *sdcp = global_sdcp;
@@ -244,6 +307,8 @@ static void dcp_perform_op(struct dcp_op *op)
 
 	/* submit the work */
 	mutex_lock(mutex);
+	dcp_clock(sdcp, CLOCK_ON, false);
+	sdcp->chan_in_use[chan] = true;
 
 	__raw_writel(-1, sdcp->dcp_regs_base + HW_DCP_CHnSTAT_CLR(chan));
 
@@ -272,8 +337,9 @@ static void dcp_perform_op(struct dcp_op *op)
 				__raw_readl(sdcp->dcp_regs_base +
 				HW_DCP_CHnSTAT(chan)) & 0xff);
 out:
+	sdcp->chan_in_use[chan] = false;
+	dcp_clock(sdcp, CLOCK_OFF, false);
 	mutex_unlock(mutex);
-
 	dma_unmap_single(sdcp->dev, pkt_phys, sizeof(*pkt), DMA_TO_DEVICE);
 }
 
@@ -1043,6 +1109,8 @@ static int dcp_sha_init(struct shash_desc *desc)
 	struct mutex *mutex = &sdcp->op_mutex[HASH_CHAN];
 
 	mutex_lock(mutex);
+	dcp_clock(sdcp, CLOCK_ON, false);
+	sdcp->chan_in_use[HASH_CHAN] = true;
 
 	op->length = 0;
 
@@ -1172,6 +1240,8 @@ static int dcp_sha_final(struct shash_desc *desc, u8 *out)
 	for (i = 0; i < digest_len; i++)
 		*out++ = *--digest;
 
+	sdcp->chan_in_use[HASH_CHAN] = false;
+	dcp_clock(sdcp, CLOCK_OFF, false);
 	mutex_unlock(mutex);
 
 	return ret;
@@ -1245,6 +1315,110 @@ static irqreturn_t dcp_irq(int irq, void *context)
 	return dcp_common_irq(irq, context);
 }
 
+/* DCP bootstream verification interface: uses OTP key for crypto */
+static int dcp_bootstream_ioctl(struct inode *inode, struct file *file,
+					 unsigned int cmd, unsigned long arg)
+{
+	struct dcp *sdcp = global_sdcp;
+	struct dcpboot_dma_area *da = sdcp->dcpboot_dma_area;
+	void __user *argp = (void __user *)arg;
+	int chan = ROM_DCP_CHAN;
+	unsigned long timeout;
+	struct mutex *mutex;
+	int retVal;
+
+	/* be paranoid */
+	if (sdcp == NULL)
+		return -EBADF;
+
+	if (cmd != DBS_ENC && cmd != DBS_DEC)
+		return -EINVAL;
+
+	/* copy to (aligned) block */
+	if (copy_from_user(da->block, argp, 16))
+		return -EFAULT;
+
+	mutex = &sdcp->op_mutex[chan];
+	mutex_lock(mutex);
+	dcp_clock(sdcp, CLOCK_ON, false);
+	sdcp->chan_in_use[chan] = true;
+
+	__raw_writel(-1, sdcp->dcp_regs_base +
+		HW_DCP_CHnSTAT_CLR(ROM_DCP_CHAN));
+	__raw_writel(BF(ROM_DCP_CHAN_MASK, DCP_STAT_IRQ),
+		sdcp->dcp_regs_base + HW_DCP_STAT_CLR);
+
+	da->hw_packet.pNext = 0;
+	da->hw_packet.pkt1 = BM_DCP_PACKET1_DECR_SEMAPHORE |
+	    BM_DCP_PACKET1_ENABLE_CIPHER | BM_DCP_PACKET1_OTP_KEY |
+	    BM_DCP_PACKET1_INTERRUPT |
+	    (cmd == DBS_ENC ? BM_DCP_PACKET1_CIPHER_ENCRYPT : 0);
+	da->hw_packet.pkt2 = BF(0, DCP_PACKET2_CIPHER_CFG) |
+	    BF(0, DCP_PACKET2_KEY_SELECT) |
+	    BF(BV_DCP_PACKET2_CIPHER_MODE__ECB, DCP_PACKET2_CIPHER_MODE) |
+	    BF(BV_DCP_PACKET2_CIPHER_SELECT__AES128, DCP_PACKET2_CIPHER_SELECT);
+	da->hw_packet.pSrc = sdcp->dcpboot_dma_area_phys +
+	    offsetof(struct dcpboot_dma_area, block);
+	da->hw_packet.pDst = da->hw_packet.pSrc;	/* in-place */
+	da->hw_packet.size = 16;
+	da->hw_packet.pPayload = 0;
+	da->hw_packet.stat = 0;
+
+	/* Load the work packet pointer and bump the channel semaphore */
+	__raw_writel(sdcp->dcpboot_dma_area_phys +
+		     offsetof(struct dcpboot_dma_area, hw_packet),
+		     sdcp->dcp_regs_base + HW_DCP_CHnCMDPTR(ROM_DCP_CHAN));
+
+	sdcp->wait[chan] = 0;
+	__raw_writel(BF(1, DCP_CHnSEMA_INCREMENT),
+		     sdcp->dcp_regs_base + HW_DCP_CHnSEMA(ROM_DCP_CHAN));
+
+	timeout = jiffies + msecs_to_jiffies(100);
+
+	while (time_before(jiffies, timeout) && sdcp->wait[chan] == 0)
+		cpu_relax();
+
+	if (!time_before(jiffies, timeout)) {
+		dev_err(sdcp->dev,
+			"Timeout while waiting for operation to complete\n");
+		retVal = -ETIMEDOUT;
+		goto exit;
+	}
+
+	if ((__raw_readl(sdcp->dcp_regs_base + HW_DCP_CHnSTAT(ROM_DCP_CHAN))
+			& 0xff) != 0) {
+		dev_err(sdcp->dev, "Channel stat error 0x%02x\n",
+			__raw_readl(sdcp->dcp_regs_base +
+				    HW_DCP_CHnSTAT(ROM_DCP_CHAN)) & 0xff);
+		retVal = -EFAULT;
+		goto exit;
+	}
+
+	if (copy_to_user(argp, da->block, 16)) {
+		retVal =  -EFAULT;
+		goto exit;
+	}
+
+	retVal = 0;
+
+exit:
+	sdcp->chan_in_use[chan] = false;
+	dcp_clock(sdcp, CLOCK_OFF, false);
+	mutex_unlock(mutex);
+	return retVal;
+}
+
+static const struct file_operations dcp_bootstream_fops = {
+	.owner =	THIS_MODULE,
+	.ioctl =	dcp_bootstream_ioctl,
+};
+
+static struct miscdevice dcp_bootstream_misc = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	.name	= "dcpboot",
+	.fops = &dcp_bootstream_fops,
+};
+
 static int dcp_probe(struct platform_device *pdev)
 {
 	struct dcp *sdcp = NULL;
@@ -1272,6 +1446,7 @@ static int dcp_probe(struct platform_device *pdev)
 	for (i = 0; i < DCP_NUM_CHANNELS; i++) {
 		mutex_init(&sdcp->op_mutex[i]);
 		init_completion(&sdcp->op_wait[i]);
+		sdcp->chan_in_use[i] = false;
 	}
 
 	platform_set_drvdata(pdev, sdcp);
@@ -1282,7 +1457,8 @@ static int dcp_probe(struct platform_device *pdev)
 		ret = -ENXIO;
 		goto err_kfree;
 	}
-	sdcp->dcp_regs_base = (u32) IO_ADDRESS(r->start);
+	sdcp->dcp_regs_base = (u32) ioremap(r->start, r->end - r->start + 1);
+	dcp_clock(sdcp, CLOCK_ON, true);
 
 	/* Soft reset and remove the clock gate */
 	__raw_writel(BM_DCP_CTRL_SFTRST, sdcp->dcp_regs_base + HW_DCP_CTRL_SET);
@@ -1318,14 +1494,14 @@ static int dcp_probe(struct platform_device *pdev)
 	if (!r) {
 		dev_err(&pdev->dev, "can't get IRQ resource (0)\n");
 		ret = -EIO;
-		goto err_kfree;
+		goto err_gate_clk;
 	}
 	sdcp->dcp_vmi_irq = r->start;
 	ret = request_irq(sdcp->dcp_vmi_irq, dcp_vmi_irq, 0, "dcp",
 				sdcp);
 	if (ret != 0) {
 		dev_err(&pdev->dev, "can't request_irq (0)\n");
-		goto err_kfree;
+		goto err_gate_clk;
 	}
 
 	r = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
@@ -1346,7 +1522,7 @@ static int dcp_probe(struct platform_device *pdev)
 	ret = crypto_register_alg(&dcp_aes_alg);
 	if (ret != 0)  {
 		dev_err(&pdev->dev, "Failed to register aes crypto\n");
-		goto err_kfree;
+		goto err_free_irq1;
 	}
 
 	ret = crypto_register_alg(&dcp_aes_ecb_alg);
@@ -1439,9 +1615,30 @@ static int dcp_probe(struct platform_device *pdev)
 		}
 	}
 
+	/* register dcpboot interface to allow apps (such as kobs-ng) to
+	 * verify files (such as the bootstream) using the OTP key for crypto */
+	ret = misc_register(&dcp_bootstream_misc);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "Unable to register misc device\n");
+		goto err_unregister_sha1;
+	}
+
+	sdcp->dcpboot_dma_area = dma_alloc_coherent(&pdev->dev,
+		sizeof(*sdcp->dcpboot_dma_area), &sdcp->dcpboot_dma_area_phys,
+		GFP_KERNEL);
+	if (sdcp->dcpboot_dma_area == NULL) {
+		dev_err(&pdev->dev,
+			"Unable to allocate DMAable memory \
+			 for dcpboot interface\n");
+		goto err_dereg;
+	}
+
+	dcp_clock(sdcp, CLOCK_OFF, false);
 	dev_notice(&pdev->dev, "DCP crypto enabled.!\n");
 	return 0;
 
+err_dereg:
+	misc_deregister(&dcp_bootstream_misc);
 err_unregister_sha1:
 	crypto_unregister_shash(&dcp_sha1_alg);
 err_unregister_aes_cbc:
@@ -1450,8 +1647,12 @@ err_unregister_aes_ecb:
 	crypto_unregister_alg(&dcp_aes_ecb_alg);
 err_unregister_aes:
 	crypto_unregister_alg(&dcp_aes_alg);
+err_free_irq1:
+	free_irq(sdcp->dcp_irq, sdcp);
 err_free_irq0:
 	free_irq(sdcp->dcp_vmi_irq, sdcp);
+err_gate_clk:
+	dcp_clock(sdcp, CLOCK_OFF, false);
 err_kfree:
 	kfree(sdcp);
 err:
@@ -1466,6 +1667,8 @@ static int dcp_remove(struct platform_device *pdev)
 	sdcp = platform_get_drvdata(pdev);
 	platform_set_drvdata(pdev, NULL);
 
+	dcp_clock(sdcp, CLOCK_ON, false);
+
 	free_irq(sdcp->dcp_irq, sdcp);
 	free_irq(sdcp->dcp_vmi_irq, sdcp);
 
@@ -1487,34 +1690,58 @@ static int dcp_remove(struct platform_device *pdev)
 			sdcp->user_buf_desc, sdcp->user_buf_desc->my_phys);
 	}
 
+	if (sdcp->dcpboot_dma_area) {
+		dma_free_coherent(&pdev->dev, sizeof(*sdcp->dcpboot_dma_area),
+			  sdcp->dcpboot_dma_area, sdcp->dcpboot_dma_area_phys);
+		misc_deregister(&dcp_bootstream_misc);
+	}
+
+
 	crypto_unregister_shash(&dcp_sha1_alg);
-	crypto_unregister_shash(&dcp_sha256_alg);
+
+	if (__raw_readl(sdcp->dcp_regs_base + HW_DCP_CAPABILITY1) &
+		BF_DCP_CAPABILITY1_HASH_ALGORITHMS(
+		BV_DCP_CAPABILITY1_HASH_ALGORITHMS__SHA256))
+		crypto_unregister_shash(&dcp_sha256_alg);
 
 	crypto_unregister_alg(&dcp_aes_cbc_alg);
 	crypto_unregister_alg(&dcp_aes_ecb_alg);
 	crypto_unregister_alg(&dcp_aes_alg);
+
+	dcp_clock(sdcp, CLOCK_OFF, true);
+	iounmap((void *) sdcp->dcp_regs_base);
 	kfree(sdcp);
 	global_sdcp = NULL;
 
 	return 0;
 }
 
-
-#ifdef CONFIG_PM
 static int dcp_suspend(struct platform_device *pdev,
 		pm_message_t state)
 {
+#ifdef CONFIG_PM
+	struct dcp *sdcp = platform_get_drvdata(pdev);
+
+	if (sdcp->clock_state == CLOCK_ON) {
+		dcp_clock(sdcp, CLOCK_OFF, true);
+		/* indicate that clock needs to be turned on upon resume */
+		sdcp->clock_state = CLOCK_ON;
+	}
+#endif
 	return 0;
 }
 
 static int dcp_resume(struct platform_device *pdev)
 {
+#ifdef CONFIG_PM
+	struct dcp *sdcp = platform_get_drvdata(pdev);
+
+	/* if clock was on prior to suspend, turn it back on */
+	if (sdcp->clock_state == CLOCK_ON)
+		dcp_clock(sdcp, CLOCK_ON, true);
+#endif
 	return 0;
 }
-#else
-#define dcp_suspend	NULL
-#define	dcp_resume	NULL
-#endif
 
 static struct platform_driver dcp_driver = {
 	.probe		= dcp_probe,
diff --git a/drivers/crypto/dcp.h b/drivers/crypto/dcp.h
index 00cd27b479c0..a4db91334d06 100644
--- a/drivers/crypto/dcp.h
+++ b/drivers/crypto/dcp.h
@@ -19,7 +19,12 @@
 #define HASH_CHAN	0
 #define HASH_MASK	(1 << HASH_CHAN)
 
-#define ALL_MASK	(CIPHER_MASK | HASH_MASK)
+/* DCP boostream interface uses this channel (same as the ROM) */
+#define ROM_DCP_CHAN 3
+#define ROM_DCP_CHAN_MASK (1 << ROM_DCP_CHAN)
+
+
+#define ALL_MASK	(CIPHER_MASK | HASH_MASK | ROM_DCP_CHAN_MASK)
 
 /* Defines the initialization value for the dcp control register */
 #define DCP_CTRL_INIT \
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index 81e1020fb514..27e06ebc2a59 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -89,6 +89,15 @@ config MX3_IPU_IRQS
 	  To avoid bloating the irq_desc[] array we allocate a sufficient
 	  number of IRQ slots and map them dynamically to specific sources.
 
+config MXC_PXP
+    bool "MXC PxP support"
+    select DMA_ENGINE
+
+config MXC_PXP_CLIENT_DEVICE
+    bool "MXC PxP Client Device"
+    default y
+    depends on MXC_PXP
+
 config TXX9_DMAC
 	tristate "Toshiba TXx9 SoC DMA support"
 	depends on MACH_TX49XX || MACH_TX39XX
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index 40e1e0083571..72c212ac6b79 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -9,4 +9,5 @@ obj-$(CONFIG_MV_XOR) += mv_xor.o
 obj-$(CONFIG_DW_DMAC) += dw_dmac.o
 obj-$(CONFIG_AT_HDMAC) += at_hdmac.o
 obj-$(CONFIG_MX3_IPU) += ipu/
+obj-$(CONFIG_MXC_PXP) += pxp/
 obj-$(CONFIG_TXX9_DMAC) += txx9dmac.o
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index e3235ff8e551..8bb795e7d7b2 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -68,10 +68,19 @@ static u8 *i2c_buf_virt;
 
 static void hw_i2c_dmachan_reset(struct mxs_i2c_dev *dev)
 {
+	mxs_dma_disable(dev->dma_chan);
 	mxs_dma_reset(dev->dma_chan);
 	mxs_dma_ack_irq(dev->dma_chan);
 }
 
+static mxs_i2c_reset(struct mxs_i2c_dev *mxs_i2c)
+{
+	hw_i2c_dmachan_reset(mxs_i2c);
+	mxs_dma_enable_irq(mxs_i2c->dma_chan, 1);
+	mxs_reset_block((void __iomem *)mxs_i2c->regbase, 0);
+	__raw_writel(0x0000FF00, mxs_i2c->regbase + HW_I2C_CTRL1_SET);
+}
+
 static int hw_i2c_dma_init(struct platform_device *pdev)
 {
 	struct mxs_i2c_dev *mxs_i2c = platform_get_drvdata(pdev);
@@ -159,7 +168,7 @@ static void hw_i2c_dma_setup_read(u8 addr, void *buff, int len, int flags)
 	desc[0]->cmd.cmd.bits.pio_words = 1;
 	desc[0]->cmd.cmd.bits.wait4end = 1;
 	desc[0]->cmd.cmd.bits.dec_sem = 1;
-	desc[0]->cmd.cmd.bits.irq = 1;
+	desc[0]->cmd.cmd.bits.irq = 0;
 	desc[0]->cmd.cmd.bits.chain = 1;
 	desc[0]->cmd.cmd.bits.command = DMA_READ;
 	desc[0]->cmd.address = i2c_buf_phys;
@@ -319,6 +328,7 @@ static int mxs_i2c_xfer_msg(struct i2c_adapter *adap,
 							msecs_to_jiffies(1000)
 	    );
 	if (err <= 0) {
+		mxs_i2c_reset(dev);
 		dev_dbg(dev->dev, "controller is timed out\n");
 		return -ETIMEDOUT;
 	}
@@ -365,9 +375,19 @@ static irqreturn_t mxs_i2c_dma_isr(int this_irq, void *dev_id)
 	mxs_dma_ack_irq(mxs_i2c->dma_chan);
 	mxs_dma_cooked(mxs_i2c->dma_chan, &list);
 
+	complete(&mxs_i2c->cmd_complete);
+
 	return IRQ_HANDLED;
 }
 
+static void mxs_i2c_task(struct work_struct *work)
+{
+	struct mxs_i2c_dev *mxs_i2c = container_of(work,
+					struct mxs_i2c_dev, work);
+	mxs_i2c_reset(mxs_i2c);
+	complete(&mxs_i2c->cmd_complete);
+}
+
 #define I2C_IRQ_MASK 0x000000FF
 static irqreturn_t mxs_i2c_isr(int this_irq, void *dev_id)
 {
@@ -382,20 +402,8 @@ static irqreturn_t mxs_i2c_isr(int this_irq, void *dev_id)
 
 	if (stat & BM_I2C_CTRL1_NO_SLAVE_ACK_IRQ) {
 		mxs_i2c->cmd_err = -EREMOTEIO;
-
-		/*
-		 * Stop DMA
-		 * Clear NAK
-		 */
-		__raw_writel(BM_I2C_CTRL1_CLR_GOT_A_NAK,
-			     mxs_i2c->regbase + HW_I2C_CTRL1_SET);
-		hw_i2c_dmachan_reset(mxs_i2c);
-		mxs_reset_block((void __iomem *)mxs_i2c->regbase, 1);
-		/* Will catch all error (IRQ mask) */
-		__raw_writel(0x0000FF00, mxs_i2c->regbase + HW_I2C_CTRL1_SET);
-
-		complete(&mxs_i2c->cmd_complete);
-
+		/* it takes long time to reset i2c */
+		schedule_work(&mxs_i2c->work);
 		goto done;
 	}
 
@@ -407,7 +415,10 @@ static irqreturn_t mxs_i2c_isr(int this_irq, void *dev_id)
 		complete(&mxs_i2c->cmd_complete);
 		goto done;
 	}
-	if ((stat & done_mask) == done_mask)
+
+	if ((stat & done_mask) == done_mask &&
+		(mxs_i2c->flags & MXS_I2C_PIOQUEUE_MODE))
+
 		complete(&mxs_i2c->cmd_complete);
 
 done:
@@ -524,6 +535,8 @@ static int mxs_i2c_probe(struct platform_device *pdev)
 
 	}
 
+	INIT_WORK(&mxs_i2c->work, mxs_i2c_task);
+
 	return 0;
 
 no_i2c_adapter:
diff --git a/drivers/i2c/busses/i2c-mxs.h b/drivers/i2c/busses/i2c-mxs.h
index 4ddca007624a..1a35385b793b 100644
--- a/drivers/i2c/busses/i2c-mxs.h
+++ b/drivers/i2c/busses/i2c-mxs.h
@@ -37,5 +37,6 @@ struct mxs_i2c_dev {
 	struct i2c_adapter	adapter;
 	spinlock_t		lock;
 	wait_queue_head_t	queue;
+	struct work_struct 	work;
 };
 #endif
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 1148140d08a1..5598ecb48c5b 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -19,6 +19,9 @@
 #include <linux/input.h>
 #include <linux/major.h>
 #include <linux/device.h>
+#ifdef CONFIG_WAKELOCK
+#include <linux/wakelock.h>
+#endif
 #include "input-compat.h"
 
 struct evdev {
@@ -42,6 +45,10 @@ struct evdev_client {
 	struct fasync_struct *fasync;
 	struct evdev *evdev;
 	struct list_head node;
+#ifdef CONFIG_WAKELOCK
+	struct wake_lock wake_lock;
+	char name[28];
+#endif
 };
 
 static struct evdev *evdev_table[EVDEV_MINORS];
@@ -54,6 +61,9 @@ static void evdev_pass_event(struct evdev_client *client,
 	 * Interrupts are disabled, just acquire the lock
 	 */
 	spin_lock(&client->buffer_lock);
+#ifdef CONFIG_WAKELOCK
+	wake_lock_timeout(&client->wake_lock, 5 * HZ);
+#endif
 	client->buffer[client->head++] = *event;
 	client->head &= EVDEV_BUFFER_SIZE - 1;
 	spin_unlock(&client->buffer_lock);
@@ -70,8 +80,15 @@ static void evdev_event(struct input_handle *handle,
 	struct evdev *evdev = handle->private;
 	struct evdev_client *client;
 	struct input_event event;
+#ifdef CONFIG_WAKELOCK
+	struct timespec ts;
 
+	ktime_get_ts(&ts);
+	event.time.tv_sec = ts.tv_sec;
+	event.time.tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+#else
 	do_gettimeofday(&event.time);
+#endif
 	event.type = type;
 	event.code = code;
 	event.value = value;
@@ -232,6 +249,9 @@ static int evdev_release(struct inode *inode, struct file *file)
 	mutex_unlock(&evdev->mutex);
 
 	evdev_detach_client(evdev, client);
+#ifdef CONFIG_WAKELOCK
+	wake_lock_destroy(&client->wake_lock);
+#endif
 	kfree(client);
 
 	evdev_close_device(evdev);
@@ -268,6 +288,11 @@ static int evdev_open(struct inode *inode, struct file *file)
 	}
 
 	spin_lock_init(&client->buffer_lock);
+#ifdef CONFIG_WAKELOCK
+	snprintf(client->name, sizeof(client->name), "%s-%d", dev_name(&evdev->dev),
+			task_tgid_vnr(current));
+	wake_lock_init(&client->wake_lock, WAKE_LOCK_SUSPEND, client->name);
+#endif
 	client->evdev = evdev;
 	evdev_attach_client(evdev, client);
 
@@ -331,6 +356,10 @@ static int evdev_fetch_next_event(struct evdev_client *client,
 	if (have_event) {
 		*event = client->buffer[client->tail++];
 		client->tail &= EVDEV_BUFFER_SIZE - 1;
+#ifdef CONFIG_WAKELOCK
+		if (client->head == client->tail)
+			wake_unlock(&client->wake_lock);
+#endif
 	}
 
 	spin_unlock_irq(&client->buffer_lock);
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index fea01a591076..88e2a24d9cff 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -31,7 +31,7 @@ obj-$(CONFIG_KEYBOARD_STOWAWAY)		+= stowaway.o
 obj-$(CONFIG_KEYBOARD_SUNKBD)		+= sunkbd.o
 obj-$(CONFIG_KEYBOARD_TOSA)		+= tosakbd.o
 obj-$(CONFIG_KEYBOARD_XTKBD)		+= xtkbd.o
-obj-$(CONFIG_KEYBOARD_MXC)		+= mxc_keyb.o
+obj-$(CONFIG_KEYBOARD_MXC)		+= mxc_keyb.o mxc_pwrkey.o
 obj-$(CONFIG_KEYBOARD_MPR084)		+= mpr084.o
 obj-$(CONFIG_KEYBOARD_STMP3XXX)		+= stmp3xxx-kbd.o
 obj-$(CONFIG_KEYBOARD_MXS)		+= mxs-kbd.o
diff --git a/drivers/input/keyboard/mxc_keyb.c b/drivers/input/keyboard/mxc_keyb.c
index 99dd7cf51cb5..bd5ecd8c20f1 100644
--- a/drivers/input/keyboard/mxc_keyb.c
+++ b/drivers/input/keyboard/mxc_keyb.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -58,12 +58,168 @@
 #include <linux/clk.h>
 #include <asm/mach/keypad.h>
 
+/*!
+ * Keypad Module Name
+ */
+#define MOD_NAME  "mxckpd"
+
+/*!
+ * XLATE mode selection
+ */
+#define KEYPAD_XLATE        0
+
+/*!
+ * RAW mode selection
+ */
+#define KEYPAD_RAW          1
+
+/*!
+ * Maximum number of keys.
+ */
+#define MAXROW			8
+#define MAXCOL			8
+#define MXC_MAXKEY		(MAXROW * MAXCOL)
+
+/*!
+ * This define indicates break scancode for every key release. A constant
+ * of 128 is added to the key press scancode.
+ */
+#define  MXC_KEYRELEASE   128
+
+/*
+ * _reg_KPP_KPCR   _reg_KPP_KPSR _reg_KPP_KDDR _reg_KPP_KPDR
+ *  The offset of Keypad Control Register Address
+ */
+#define KPCR    0x00
+
+/*
+ * The offset of Keypad Status Register Address
+ */
+#define KPSR    0x02
+
+/*
+ * The offset of Keypad Data Direction Address
+ */
+#define KDDR   0x04
+
+/*
+ * The offset of Keypad Data Register
+ */
+#define KPDR    0x06
+
+/*
+ * Key Press Interrupt Status bit
+ */
+#define KBD_STAT_KPKD        0x01
+
+/*
+ * Key Release Interrupt Status bit
+ */
+#define KBD_STAT_KPKR        0x02
+
+/*
+ * Key Depress Synchronizer Chain Status bit
+ */
+#define KBD_STAT_KDSC        0x04
+
+/*
+ * Key Release Synchronizer Status bit
+ */
+#define KBD_STAT_KRSS        0x08
+
+/*
+ * Key Depress Interrupt Enable Status bit
+ */
+#define KBD_STAT_KDIE        0x100
+
 /*
- * Module header file
+ * Key Release Interrupt Enable
  */
-#include "mxc_keyb.h"
+#define KBD_STAT_KRIE        0x200
+
+/*
+ * Keypad Clock Enable
+ */
+#define KBD_STAT_KPPEN       0x400
+
+/*!
+ * Buffer size of keypad queue. Should be a power of 2.
+ */
+#define KPP_BUF_SIZE    128
+
+/*!
+ * Test whether bit is set for integer c
+ */
+#define TEST_BIT(c, n) ((c) & (0x1 << (n)))
+
+/*!
+ * Set nth bit in the integer c
+ */
+#define BITSET(c, n)   ((c) | (1 << (n)))
+
+/*!
+ * Reset nth bit in the integer c
+ */
+#define BITRESET(c, n) ((c) & ~(1 << (n)))
+
+/*!
+ * This enum represents the keypad state machine to maintain debounce logic
+ * for key press/release.
+ */
+enum KeyState {
+
+	/*!
+	 * Key press state.
+	 */
+	KStateUp,
+
+	/*!
+	 * Key press debounce state.
+	 */
+	KStateFirstDown,
+
+	/*!
+	 * Key release state.
+	 */
+	KStateDown,
+
+	/*!
+	 * Key release debounce state.
+	 */
+	KStateFirstUp
+};
 
 /*!
+ * Keypad Private Data Structure
+ */
+struct keypad_priv {
+
+	/*!
+	 * Keypad state machine.
+	 */
+	enum KeyState iKeyState;
+
+	/*!
+	 * Number of rows configured in the keypad matrix
+	 */
+	unsigned long kpp_rows;
+
+	/*!
+	 * Number of Columns configured in the keypad matrix
+	 */
+	unsigned long kpp_cols;
+
+	/*!
+	 * Timer used for Keypad polling.
+	 */
+	struct timer_list poll_timer;
+
+	/*!
+	 * The base address
+	 */
+	void __iomem *base;
+};
+/*!
  * This structure holds the keypad private data structure.
  */
 static struct keypad_priv kpp_dev;
@@ -269,26 +425,26 @@ static int mxc_kpp_scan_matrix(void)
 
 	for (col = 0; col < kpp_dev.kpp_cols; col++) {	/* Col */
 		/* 2. Write 1.s to KPDR[15:8] setting column data to 1.s */
-		reg_val = __raw_readw(KPDR);
+		reg_val = __raw_readw(kpp_dev.base + KPDR);
 		reg_val |= 0xff00;
-		__raw_writew(reg_val, KPDR);
+		__raw_writew(reg_val, kpp_dev.base + KPDR);
 
 		/*
 		 * 3. Configure columns as totem pole outputs(for quick
 		 * discharging of keypad capacitance)
 		 */
-		reg_val = __raw_readw(KPCR);
+		reg_val = __raw_readw(kpp_dev.base + KPCR);
 		reg_val &= 0x00ff;
-		__raw_writew(reg_val, KPCR);
+		__raw_writew(reg_val, kpp_dev.base + KPCR);
 
 		udelay(2);
 
 		/*
 		 * 4. Configure columns as open-drain
 		 */
-		reg_val = __raw_readw(KPCR);
+		reg_val = __raw_readw(kpp_dev.base + KPCR);
 		reg_val |= ((1 << kpp_dev.kpp_cols) - 1) << 8;
-		__raw_writew(reg_val, KPCR);
+		__raw_writew(reg_val, kpp_dev.base + KPCR);
 
 		/*
 		 * 5. Write a single column to 0, others to 1.
@@ -298,9 +454,9 @@ static int mxc_kpp_scan_matrix(void)
 		 */
 
 		/* Col bit starts at 8th bit in KPDR */
-		reg_val = __raw_readw(KPDR);
+		reg_val = __raw_readw(kpp_dev.base + KPDR);
 		reg_val &= ~(1 << (8 + col));
-		__raw_writew(reg_val, KPDR);
+		__raw_writew(reg_val, kpp_dev.base + KPDR);
 
 		/* Delay added to avoid propagating the 0 from column to row
 		 * when scanning. */
@@ -308,7 +464,7 @@ static int mxc_kpp_scan_matrix(void)
 		udelay(5);
 
 		/* Read row input */
-		reg_val = __raw_readw(KPDR);
+		reg_val = __raw_readw(kpp_dev.base + KPDR);
 		for (row = 0; row < kpp_dev.kpp_rows; row++) {	/* sample row */
 			if (TEST_BIT(reg_val, row) == 0) {
 				cur_rcmap[row] = BITSET(cur_rcmap[row], col);
@@ -324,12 +480,12 @@ static int mxc_kpp_scan_matrix(void)
 	 * clear the KPKD synchronizer chain by writing "1" to KDSC register
 	 */
 	reg_val = 0x00;
-	__raw_writew(reg_val, KPDR);
-	reg_val = __raw_readw(KPDR);
-	reg_val = __raw_readw(KPSR);
+	__raw_writew(reg_val, kpp_dev.base + KPDR);
+	reg_val = __raw_readw(kpp_dev.base + KPDR);
+	reg_val = __raw_readw(kpp_dev.base + KPSR);
 	reg_val |= KBD_STAT_KPKD | KBD_STAT_KPKR | KBD_STAT_KRSS |
 	    KBD_STAT_KDSC;
-	__raw_writew(reg_val, KPSR);
+	__raw_writew(reg_val, kpp_dev.base + KPSR);
 
 	/* Check key press status change */
 
@@ -558,14 +714,14 @@ static void mxc_kpp_handle_timer(unsigned long data)
 		 * Stop scanning and wait for interrupt.
 		 * Enable press interrupt and disable release interrupt.
 		 */
-		__raw_writew(0x00FF, KPDR);
-		reg_val = __raw_readw(KPSR);
+		__raw_writew(0x00FF, kpp_dev.base + KPDR);
+		reg_val = __raw_readw(kpp_dev.base + KPSR);
 		reg_val |= (KBD_STAT_KPKR | KBD_STAT_KPKD);
 		reg_val |= KBD_STAT_KRSS | KBD_STAT_KDSC;
-		__raw_writew(reg_val, KPSR);
+		__raw_writew(reg_val, kpp_dev.base + KPSR);
 		reg_val |= KBD_STAT_KDIE;
 		reg_val &= ~KBD_STAT_KRIE;
-		__raw_writew(reg_val, KPSR);
+		__raw_writew(reg_val, kpp_dev.base + KPSR);
 
 		/*
 		 * No more keys pressed... make sure unwanted key codes are
@@ -613,7 +769,7 @@ static irqreturn_t mxc_kpp_interrupt(int irq, void *dev_id)
 
 	/* Delete the polling timer */
 	del_timer(&kpp_dev.poll_timer);
-	reg_val = __raw_readw(KPSR);
+	reg_val = __raw_readw(kpp_dev.base + KPSR);
 
 	/* Check if it is key press interrupt */
 	if (reg_val & KBD_STAT_KPKD) {
@@ -621,7 +777,7 @@ static irqreturn_t mxc_kpp_interrupt(int irq, void *dev_id)
 		 * Disable key press(KDIE status bit) interrupt
 		 */
 		reg_val &= ~KBD_STAT_KDIE;
-		__raw_writew(reg_val, KPSR);
+		__raw_writew(reg_val, kpp_dev.base + KPSR);
 	} else {
 		/* spurious interrupt */
 		return IRQ_RETVAL(0);
@@ -767,6 +923,7 @@ static int mxc_kpp_probe(struct platform_device *pdev)
 	int i, irq;
 	int retval;
 	unsigned int reg_val;
+	struct resource *res;
 
 	keypad = (struct keypad_data *)pdev->dev.platform_data;
 
@@ -774,6 +931,14 @@ static int mxc_kpp_probe(struct platform_device *pdev)
 	kpp_dev.kpp_rows = keypad->rowmax;
 	key_pad_enabled = 0;
 
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	kpp_dev.base = ioremap(res->start, res->end - res->start + 1);
+	if (!kpp_dev.base)
+		return -ENOMEM;
+
 	irq = platform_get_irq(pdev, 0);
 	keypad->irq = irq;
 
@@ -793,30 +958,30 @@ static int mxc_kpp_probe(struct platform_device *pdev)
 	 * LSB nibble in KPP is for 8 rows
 	 * MSB nibble in KPP is for 8 cols
 	 */
-	reg_val = __raw_readw(KPCR);
+	reg_val = __raw_readw(kpp_dev.base + KPCR);
 	reg_val |= (1 << keypad->rowmax) - 1;	/* LSB */
 	reg_val |= ((1 << keypad->colmax) - 1) << 8;	/* MSB */
-	__raw_writew(reg_val, KPCR);
+	__raw_writew(reg_val, kpp_dev.base + KPCR);
 
 	/* Write 0's to KPDR[15:8] */
-	reg_val = __raw_readw(KPDR);
+	reg_val = __raw_readw(kpp_dev.base + KPDR);
 	reg_val &= 0x00ff;
-	__raw_writew(reg_val, KPDR);
+	__raw_writew(reg_val, kpp_dev.base + KPDR);
 
 	/* Configure columns as output, rows as input (KDDR[15:0]) */
-	reg_val = __raw_readw(KDDR);
+	reg_val = __raw_readw(kpp_dev.base + KDDR);
 	reg_val |= 0xff00;
 	reg_val &= 0xff00;
-	__raw_writew(reg_val, KDDR);
+	__raw_writew(reg_val, kpp_dev.base + KDDR);
 
-	reg_val = __raw_readw(KPSR);
+	reg_val = __raw_readw(kpp_dev.base + KPSR);
 	reg_val &= ~(KBD_STAT_KPKR | KBD_STAT_KPKD);
 	reg_val |= KBD_STAT_KPKD;
 	reg_val |= KBD_STAT_KRSS | KBD_STAT_KDSC;
-	__raw_writew(reg_val, KPSR);
+	__raw_writew(reg_val, kpp_dev.base + KPSR);
 	reg_val |= KBD_STAT_KDIE;
 	reg_val &= ~KBD_STAT_KRIE;
-	__raw_writew(reg_val, KPSR);
+	__raw_writew(reg_val, kpp_dev.base + KPSR);
 
 	has_leaning_key = keypad->learning;
 	mxckpd_keycodes = keypad->matrix;
@@ -912,7 +1077,7 @@ static int mxc_kpp_probe(struct platform_device *pdev)
 	retval = request_irq(irq, mxc_kpp_interrupt, 0, MOD_NAME, MOD_NAME);
 	if (retval) {
 		pr_debug("KPP: request_irq(%d) returned error %d\n",
-			 MXC_INT_KPP, retval);
+			 irq, retval);
 		goto err3;
 	}
 
@@ -950,16 +1115,16 @@ static int mxc_kpp_remove(struct platform_device *pdev)
 	 * Set KDIE control bit, clear KRIE control bit (avoid false release
 	 * events. Disable the keypad GPIO pins.
 	 */
-	__raw_writew(0x00, KPCR);
-	__raw_writew(0x00, KPDR);
-	__raw_writew(0x00, KDDR);
+	__raw_writew(0x00, kpp_dev.base + KPCR);
+	__raw_writew(0x00, kpp_dev.base + KPDR);
+	__raw_writew(0x00, kpp_dev.base + KDDR);
 
-	reg_val = __raw_readw(KPSR);
+	reg_val = __raw_readw(kpp_dev.base + KPSR);
 	reg_val |= KBD_STAT_KPKD;
 	reg_val &= ~KBD_STAT_KRSS;
 	reg_val |= KBD_STAT_KDIE;
 	reg_val &= ~KBD_STAT_KRIE;
-	__raw_writew(reg_val, KPSR);
+	__raw_writew(reg_val, kpp_dev.base + KPSR);
 
 	gpio_keypad_inactive();
 	clk_disable(kpp_clk);
diff --git a/drivers/input/misc/mma7455l.c b/drivers/input/misc/mma7455l.c
index 1cee2d1add04..48dca60d2cfe 100644
--- a/drivers/input/misc/mma7455l.c
+++ b/drivers/input/misc/mma7455l.c
@@ -583,6 +583,8 @@ static int __devexit mma7455l_remove(struct i2c_client *client)
 {
 	struct mma7455l_info *mma = dev_get_drvdata(&client->dev);
 
+	free_irq(client->irq, mma);
+
 	sysfs_remove_group(&client->dev.kobj, &mma7455l_attr_group);
 	input_unregister_device(mma->input_dev);
 	dev_set_drvdata(&client->dev, NULL);
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index ba9d38c3f412..0f74aeee2aea 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -17,14 +17,11 @@
  *  it under the terms of the GNU General Public License version 2 as
  *  published by the Free Software Foundation.
  */
-#include <linux/hwmon.h>
+#include <linux/device.h>
 #include <linux/init.h>
-#include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/input.h>
 #include <linux/interrupt.h>
-#include <linux/slab.h>
-#include <linux/gpio.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/ads7846.h>
 #include <asm/irq.h>
@@ -33,9 +30,7 @@
 /*
  * This code has been heavily tested on a Nokia 770, and lightly
  * tested on other ads7846 devices (OSK/Mistral, Lubbock).
- * TSC2046 is just newer ads7846 silicon.
- * Support for ads7843 tested on Atmel at91sam926x-EK.
- * Support for ads7845 has only been stubbed in.
+ * Support for ads7843 and ads7845 has only been stubbed in.
  *
  * IRQ handling needs a workaround because of a shortcoming in handling
  * edge triggered IRQs on some platforms like the OMAP1/2. These
@@ -51,8 +46,7 @@
  * files.
  */
 
-#define TS_POLL_DELAY	(1 * 1000000)	/* ns delay before the first sample */
-#define TS_POLL_PERIOD	(5 * 1000000)	/* ns delay between samples */
+#define	TS_POLL_PERIOD	msecs_to_jiffies(10)
 
 /* this driver doesn't aim at the peak continuous sample rate */
 #define	SAMPLE_BITS	(8 /*cmd*/ + 16 /*sample*/ + 2 /* before, after */)
@@ -61,77 +55,57 @@ struct ts_event {
 	/* For portability, we can't read 12 bit values using SPI (which
 	 * would make the controller deliver them as native byteorder u16
 	 * with msbs zeroed).  Instead, we read them as two 8-bit values,
-	 * *** WHICH NEED BYTESWAPPING *** and range adjustment.
+	 * which need byteswapping then range adjustment.
 	 */
-	u16	x;
-	u16	y;
-	u16	z1, z2;
-	int	ignore;
-};
-
-/*
- * We allocate this separately to avoid cache line sharing issues when
- * driver is used with DMA-based SPI controllers (like atmel_spi) on
- * systems where main memory is not DMA-coherent (most non-x86 boards).
- */
-struct ads7846_packet {
-	u8			read_x, read_y, read_z1, read_z2, pwrdown;
-	u16			dummy;		/* for the pwrdown read */
-	struct ts_event		tc;
+	__be16 x;
+	__be16 y;
+	__be16 z1, z2;
+	int    ignore;
 };
 
 struct ads7846 {
 	struct input_dev	*input;
 	char			phys[32];
-	char			name[32];
 
-	struct spi_device	*spi;
-
-#if defined(CONFIG_HWMON) || defined(CONFIG_HWMON_MODULE)
-	struct attribute_group	*attr_group;
-	struct device		*hwmon;
-#endif
+	u32			*txbuf;
+	u32			*rxbuf;
+	u8			buflen;
+	u8			skip_samples;
+	u16			rotate;
 
+	struct spi_device	*spi;
 	u16			model;
-	u16			vref_mv;
 	u16			vref_delay_usecs;
 	u16			x_plate_ohms;
 	u16			pressure_max;
 
-	bool			swap_xy;
-
-	struct ads7846_packet	*packet;
+	u8			read_x, read_y, read_z1, read_z2, pwrdown;
+	u16                     zerro;          /* to send zerros while receiving */
+	u16			dummy;		/* for the pwrdown read */
+	struct ts_event		tc;
 
-	struct spi_transfer	xfer[18];
+	struct spi_transfer	xfer[10];
 	struct spi_message	msg[5];
 	struct spi_message	*last_msg;
 	int			msg_idx;
 	int			read_cnt;
 	int			read_rep;
 	int			last_read;
+	int			skip_this_sample;
 
 	u16			debounce_max;
 	u16			debounce_tol;
 	u16			debounce_rep;
 
-	u16			penirq_recheck_delay_usecs;
-
 	spinlock_t		lock;
-	struct hrtimer		timer;
+	struct timer_list	timer;		/* P: lock */
 	unsigned		pendown:1;	/* P: lock */
 	unsigned		pending:1;	/* P: lock */
 // FIXME remove "irq_disabled"
 	unsigned		irq_disabled:1;	/* P: lock */
 	unsigned		disabled:1;
-	unsigned		is_suspended:1;
 
-	int			(*filter)(void *data, int data_idx, int *val);
-	void			*filter_data;
-	void			(*filter_cleanup)(void *data);
 	int			(*get_pendown_state)(void);
-	int			gpio_pendown;
-
-	void			(*wait_for_sync)(void);
 };
 
 /* leave chip selected when we're done, for quicker re-select? */
@@ -141,6 +115,7 @@ struct ads7846 {
 #define	CS_CHANGE(xfer)	((xfer).cs_change = 0)
 #endif
 
+
 /*--------------------------------------------------------------------------*/
 
 /* The ADS7846 has touchscreen and other sensors.
@@ -167,16 +142,19 @@ struct ads7846 {
 #define	MAX_12BIT	((1<<12)-1)
 
 /* leave ADC powered up (disables penirq) between differential samples */
-#define	READ_12BIT_DFR(x, adc, vref) (ADS_START | ADS_A2A1A0_d_ ## x \
-	| ADS_12_BIT | ADS_DFR | \
-	(adc ? ADS_PD10_ADC_ON : 0) | (vref ? ADS_PD10_REF_ON : 0))
+#define	READ_12BIT_DFR(x) (ADS_START | ADS_A2A1A0_d_ ## x \
+	| ADS_12_BIT | ADS_DFR)
+
+#define	READ_Y	(READ_12BIT_DFR(y)  | ADS_PD10_ADC_ON)
+#define	READ_Z1	(READ_12BIT_DFR(z1) | ADS_PD10_ADC_ON)
+#define	READ_Z2	(READ_12BIT_DFR(z2) | ADS_PD10_ADC_ON)
 
-#define	READ_Y(vref)	(READ_12BIT_DFR(y,  1, vref))
-#define	READ_Z1(vref)	(READ_12BIT_DFR(z1, 1, vref))
-#define	READ_Z2(vref)	(READ_12BIT_DFR(z2, 1, vref))
+#define	READ_X	(READ_12BIT_DFR(x)  | ADS_PD10_ADC_ON)
+#define	PWRDOWN	(READ_12BIT_DFR(y)  | ADS_PD10_PDOWN)	/* LAST */
 
-#define	READ_X(vref)	(READ_12BIT_DFR(x,  1, vref))
-#define	PWRDOWN		(READ_12BIT_DFR(y,  0, 0))	/* LAST */
+/* alternate ads7843 commands */
+#define	ALT_READ_Y      (READ_12BIT_DFR(y)  | ADS_PD10_ALL_ON)
+#define	ALT_READ_X      (READ_12BIT_DFR(x)  | ADS_PD10_ALL_ON)
 
 /* single-ended samples need to first power up reference voltage;
  * we leave both ADC and VREF powered
@@ -184,15 +162,21 @@ struct ads7846 {
 #define	READ_12BIT_SER(x) (ADS_START | ADS_A2A1A0_ ## x \
 	| ADS_12_BIT | ADS_SER)
 
-#define	REF_ON	(READ_12BIT_DFR(x, 1, 1))
-#define	REF_OFF	(READ_12BIT_DFR(y, 0, 0))
+#define	REF_ON	(READ_12BIT_DFR(x) | ADS_PD10_ALL_ON)
+#define	REF_OFF	(READ_12BIT_DFR(y) | ADS_PD10_PDOWN)
+
+#define	MAX_BUF_SAMPLE_LEN	(20)
+/* Following configuration should be done in the platform configuration */
+#define	SCREEN_LANDSCAPE	1
+#undef	SCREEN_PORTRAIT
+#define MAX_DIFF_BETWEEN_SAMPLES_X	100
+#define MAX_DIFF_BETWEEN_SAMPLES_Y	100
+
 
 /*--------------------------------------------------------------------------*/
 
 /*
  * Non-touchscreen sensors only use single-ended conversions.
- * The range is GND..vREF. The ads7843 and ads7835 must use external vREF;
- * ads7846 lets that pin be unconnected, to use internal vREF.
  */
 
 struct ser_req {
@@ -200,6 +184,7 @@ struct ser_req {
 	u8			command;
 	u8			ref_off;
 	u16			scratch;
+	u16                     zerro;
 	__be16			sample;
 	struct spi_message	msg;
 	struct spi_transfer	xfer[6];
@@ -211,247 +196,132 @@ static void ads7846_disable(struct ads7846 *ts);
 static int device_suspended(struct device *dev)
 {
 	struct ads7846 *ts = dev_get_drvdata(dev);
-	return ts->is_suspended || ts->disabled;
+	return dev->power.power_state.event != PM_EVENT_ON || ts->disabled;
 }
 
+static int ads7843_setup_buffers(struct device *dev)
+{
+	struct ads7846		*ts = dev_get_drvdata(dev);
+	int i;
+
+	ts->txbuf = kzalloc(sizeof(u32) * ts->buflen * 3, GFP_KERNEL);
+	if (!ts->txbuf)
+		return -ENOMEM;
+
+	ts->rxbuf = kzalloc(sizeof(u32) * ts->buflen * 3, GFP_KERNEL);
+	if (!ts->rxbuf) {
+		kfree(ts->txbuf);
+		return -ENOMEM;
+	}
+	for (i = 0; i < ((ts->buflen * 3) / 2); i++)
+#if defined( SCREEN_LANDSCAPE )
+		ts->txbuf[i] = (READ_12BIT_DFR(x) | ADS_PD10_PDOWN) << 8;
+#else
+		ts->txbuf[i] = (READ_12BIT_DFR(y) | ADS_PD10_PDOWN) << 8;
+#endif
+	for (; i < ts->buflen * 3; i++)
+#if defined( SCREEN_LANDSCAPE )
+		ts->txbuf[i] = (READ_12BIT_DFR(y) | ADS_PD10_PDOWN) << 8;
+#else
+		ts->txbuf[i] = (READ_12BIT_DFR(x) | ADS_PD10_PDOWN) << 8;
+#endif
+	return 0;
+}
+
+
 static int ads7846_read12_ser(struct device *dev, unsigned command)
 {
 	struct spi_device	*spi = to_spi_device(dev);
 	struct ads7846		*ts = dev_get_drvdata(dev);
 	struct ser_req		*req = kzalloc(sizeof *req, GFP_KERNEL);
 	int			status;
-	int			use_internal;
+	int			sample;
+	int			i;
 
 	if (!req)
 		return -ENOMEM;
 
 	spi_message_init(&req->msg);
 
-	/* FIXME boards with ads7846 might use external vref instead ... */
-	use_internal = (ts->model == 7846);
-
-	/* maybe turn on internal vREF, and let it settle */
-	if (use_internal) {
-		req->ref_on = REF_ON;
-		req->xfer[0].tx_buf = &req->ref_on;
-		req->xfer[0].len = 1;
-		spi_message_add_tail(&req->xfer[0], &req->msg);
-
-		req->xfer[1].rx_buf = &req->scratch;
-		req->xfer[1].len = 2;
-
-		/* for 1uF, settle for 800 usec; no cap, 100 usec.  */
-		req->xfer[1].delay_usecs = ts->vref_delay_usecs;
-		spi_message_add_tail(&req->xfer[1], &req->msg);
-	}
+	/* activate reference, so it has time to settle; */
+	req->ref_on = REF_ON;
+	req->xfer[0].tx_buf = &req->ref_on;
+	req->xfer[0].len = 1;
+	req->xfer[1].tx_buf = &req->zerro;
+	req->xfer[1].rx_buf = &req->scratch;
+	req->xfer[1].len = 2;
+
+	/*
+	 * for external VREF, 0 usec (and assume it's always on);
+	 * for 1uF, use 800 usec;
+	 * no cap, 100 usec.
+	 */
+	req->xfer[1].delay_usecs = ts->vref_delay_usecs;
 
 	/* take sample */
 	req->command = (u8) command;
 	req->xfer[2].tx_buf = &req->command;
 	req->xfer[2].len = 1;
-	spi_message_add_tail(&req->xfer[2], &req->msg);
-
+	req->xfer[3].tx_buf = &req->zerro;
 	req->xfer[3].rx_buf = &req->sample;
 	req->xfer[3].len = 2;
-	spi_message_add_tail(&req->xfer[3], &req->msg);
 
 	/* REVISIT:  take a few more samples, and compare ... */
 
-	/* converter in low power mode & enable PENIRQ */
-	req->ref_off = PWRDOWN;
+	/* turn off reference */
+	req->ref_off = REF_OFF;
 	req->xfer[4].tx_buf = &req->ref_off;
 	req->xfer[4].len = 1;
-	spi_message_add_tail(&req->xfer[4], &req->msg);
-
+	// TODO req->xfer[3].tx_buf = &req->zerro;
+	req->xfer[5].tx_buf = &req->zerro;
 	req->xfer[5].rx_buf = &req->scratch;
 	req->xfer[5].len = 2;
+
 	CS_CHANGE(req->xfer[5]);
-	spi_message_add_tail(&req->xfer[5], &req->msg);
+
+	/* group all the transfers together, so we can't interfere with
+	 * reading touchscreen state; disable penirq while sampling
+	 */
+	for (i = 0; i < 6; i++)
+		spi_message_add_tail(&req->xfer[i], &req->msg);
 
 	ts->irq_disabled = 1;
-	disable_irq(spi->irq);
+	disable_irq_nosync(spi->irq);
 	status = spi_sync(spi, &req->msg);
 	ts->irq_disabled = 0;
 	enable_irq(spi->irq);
 
-	if (status == 0) {
-		/* on-wire is a must-ignore bit, a BE12 value, then padding */
-		status = be16_to_cpu(req->sample);
-		status = status >> 3;
-		status &= 0x0fff;
-	}
+	if (req->msg.status)
+		status = req->msg.status;
+
+	/* on-wire is a must-ignore bit, a BE12 value, then padding */
+	sample = be16_to_cpu(req->sample);
+	sample = sample >> 3;
+	sample &= 0x0fff;
 
 	kfree(req);
-	return status;
+	return status ? status : sample;
 }
 
-#if defined(CONFIG_HWMON) || defined(CONFIG_HWMON_MODULE)
-
-#define SHOW(name, var, adjust) static ssize_t \
+#define SHOW(name) static ssize_t \
 name ## _show(struct device *dev, struct device_attribute *attr, char *buf) \
 { \
-	struct ads7846 *ts = dev_get_drvdata(dev); \
 	ssize_t v = ads7846_read12_ser(dev, \
-			READ_12BIT_SER(var) | ADS_PD10_ALL_ON); \
+			READ_12BIT_SER(name) | ADS_PD10_ALL_ON); \
 	if (v < 0) \
 		return v; \
-	return sprintf(buf, "%u\n", adjust(ts, v)); \
+	return sprintf(buf, "%u\n", (unsigned) v); \
 } \
 static DEVICE_ATTR(name, S_IRUGO, name ## _show, NULL);
 
-
-/* Sysfs conventions report temperatures in millidegrees Celsius.
- * ADS7846 could use the low-accuracy two-sample scheme, but can't do the high
- * accuracy scheme without calibration data.  For now we won't try either;
- * userspace sees raw sensor values, and must scale/calibrate appropriately.
- */
-static inline unsigned null_adjust(struct ads7846 *ts, ssize_t v)
-{
-	return v;
-}
-
-SHOW(temp0, temp0, null_adjust)		/* temp1_input */
-SHOW(temp1, temp1, null_adjust)		/* temp2_input */
-
-
-/* sysfs conventions report voltages in millivolts.  We can convert voltages
- * if we know vREF.  userspace may need to scale vAUX to match the board's
- * external resistors; we assume that vBATT only uses the internal ones.
- */
-static inline unsigned vaux_adjust(struct ads7846 *ts, ssize_t v)
-{
-	unsigned retval = v;
-
-	/* external resistors may scale vAUX into 0..vREF */
-	retval *= ts->vref_mv;
-	retval = retval >> 12;
-	return retval;
-}
-
-static inline unsigned vbatt_adjust(struct ads7846 *ts, ssize_t v)
-{
-	unsigned retval = vaux_adjust(ts, v);
-
-	/* ads7846 has a resistor ladder to scale this signal down */
-	if (ts->model == 7846)
-		retval *= 4;
-	return retval;
-}
-
-SHOW(in0_input, vaux, vaux_adjust)
-SHOW(in1_input, vbatt, vbatt_adjust)
-
-
-static struct attribute *ads7846_attributes[] = {
-	&dev_attr_temp0.attr,
-	&dev_attr_temp1.attr,
-	&dev_attr_in0_input.attr,
-	&dev_attr_in1_input.attr,
-	NULL,
-};
-
-static struct attribute_group ads7846_attr_group = {
-	.attrs = ads7846_attributes,
-};
-
-static struct attribute *ads7843_attributes[] = {
-	&dev_attr_in0_input.attr,
-	&dev_attr_in1_input.attr,
-	NULL,
-};
-
-static struct attribute_group ads7843_attr_group = {
-	.attrs = ads7843_attributes,
-};
-
-static struct attribute *ads7845_attributes[] = {
-	&dev_attr_in0_input.attr,
-	NULL,
-};
-
-static struct attribute_group ads7845_attr_group = {
-	.attrs = ads7845_attributes,
-};
-
-static int ads784x_hwmon_register(struct spi_device *spi, struct ads7846 *ts)
-{
-	struct device *hwmon;
-	int err;
-
-	/* hwmon sensors need a reference voltage */
-	switch (ts->model) {
-	case 7846:
-		if (!ts->vref_mv) {
-			dev_dbg(&spi->dev, "assuming 2.5V internal vREF\n");
-			ts->vref_mv = 2500;
-		}
-		break;
-	case 7845:
-	case 7843:
-		if (!ts->vref_mv) {
-			dev_warn(&spi->dev,
-				"external vREF for ADS%d not specified\n",
-				ts->model);
-			return 0;
-		}
-		break;
-	}
-
-	/* different chips have different sensor groups */
-	switch (ts->model) {
-	case 7846:
-		ts->attr_group = &ads7846_attr_group;
-		break;
-	case 7845:
-		ts->attr_group = &ads7845_attr_group;
-		break;
-	case 7843:
-		ts->attr_group = &ads7843_attr_group;
-		break;
-	default:
-		dev_dbg(&spi->dev, "ADS%d not recognized\n", ts->model);
-		return 0;
-	}
-
-	err = sysfs_create_group(&spi->dev.kobj, ts->attr_group);
-	if (err)
-		return err;
-
-	hwmon = hwmon_device_register(&spi->dev);
-	if (IS_ERR(hwmon)) {
-		sysfs_remove_group(&spi->dev.kobj, ts->attr_group);
-		return PTR_ERR(hwmon);
-	}
-
-	ts->hwmon = hwmon;
-	return 0;
-}
-
-static void ads784x_hwmon_unregister(struct spi_device *spi,
-				     struct ads7846 *ts)
-{
-	if (ts->hwmon) {
-		sysfs_remove_group(&spi->dev.kobj, ts->attr_group);
-		hwmon_device_unregister(ts->hwmon);
-	}
-}
-
-#else
-static inline int ads784x_hwmon_register(struct spi_device *spi,
-					 struct ads7846 *ts)
-{
-	return 0;
-}
-
-static inline void ads784x_hwmon_unregister(struct spi_device *spi,
-					    struct ads7846 *ts)
-{
-}
-#endif
+SHOW(temp0)
+SHOW(temp1)
+SHOW(vaux)
+SHOW(vbatt)
 
 static int is_pen_down(struct device *dev)
 {
-	struct ads7846	*ts = dev_get_drvdata(dev);
+	struct ads7846		*ts = dev_get_drvdata(dev);
 
 	return ts->pendown;
 }
@@ -477,11 +347,10 @@ static ssize_t ads7846_disable_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ads7846 *ts = dev_get_drvdata(dev);
-	unsigned long i;
-
-	if (strict_strtoul(buf, 10, &i))
-		return -EINVAL;
+	char *endp;
+	int i;
 
+	i = simple_strtoul(buf, &endp, 10);
 	spin_lock_irq(&ts->lock);
 
 	if (i)
@@ -496,30 +365,8 @@ static ssize_t ads7846_disable_store(struct device *dev,
 
 static DEVICE_ATTR(disable, 0664, ads7846_disable_show, ads7846_disable_store);
 
-static struct attribute *ads784x_attributes[] = {
-	&dev_attr_pen_down.attr,
-	&dev_attr_disable.attr,
-	NULL,
-};
-
-static struct attribute_group ads784x_attr_group = {
-	.attrs = ads784x_attributes,
-};
-
 /*--------------------------------------------------------------------------*/
 
-static int get_pendown_state(struct ads7846 *ts)
-{
-	if (ts->get_pendown_state)
-		return ts->get_pendown_state();
-
-	return !gpio_get_value(ts->gpio_pendown);
-}
-
-static void null_wait_for_sync(void)
-{
-}
-
 /*
  * PENIRQ only kicks the timer.  The timer only reissues the SPI transfer,
  * to retrieve touchscreen status.
@@ -531,25 +378,25 @@ static void null_wait_for_sync(void)
 static void ads7846_rx(void *ads)
 {
 	struct ads7846		*ts = ads;
-	struct ads7846_packet	*packet = ts->packet;
+	struct input_dev	*input_dev = ts->input;
 	unsigned		Rt;
+	unsigned		sync = 0;
 	u16			x, y, z1, z2;
+	unsigned long		flags;
 
-	/* ads7846_rx_val() did in-place conversion (including byteswap) from
-	 * on-the-wire format as part of debouncing to get stable readings.
+	/* adjust:  on-wire is a must-ignore bit, a BE12 value, then padding;
+	 * built from two 8 bit values written msb-first.
 	 */
-	x = packet->tc.x;
-	y = packet->tc.y;
-	z1 = packet->tc.z1;
-	z2 = packet->tc.z2;
+	x = (be16_to_cpu(ts->tc.x) >> 3) & 0x0fff;
+	y = (be16_to_cpu(ts->tc.y) >> 3) & 0x0fff;
+	z1 = (be16_to_cpu(ts->tc.z1) >> 3) & 0x0fff;
+	z2 = (be16_to_cpu(ts->tc.z2) >> 3) & 0x0fff;
 
 	/* range filtering */
 	if (x == MAX_12BIT)
 		x = 0;
 
-	if (ts->model == 7843) {
-		Rt = ts->pressure_max / 2;
-	} else if (likely(x && z1)) {
+	if (likely(x && z1 && !device_suspended(&ts->spi->dev))) {
 		/* compute touch pressure resistance using equation #2 */
 		Rt = z2;
 		Rt -= z1;
@@ -557,194 +404,281 @@ static void ads7846_rx(void *ads)
 		Rt *= ts->x_plate_ohms;
 		Rt /= z1;
 		Rt = (Rt + 2047) >> 12;
-	} else {
+	} else
 		Rt = 0;
-	}
 
 	/* Sample found inconsistent by debouncing or pressure is beyond
-	 * the maximum. Don't report it to user space, repeat at least
-	 * once more the measurement
-	 */
-	if (packet->tc.ignore || Rt > ts->pressure_max) {
-#ifdef VERBOSE
-		pr_debug("%s: ignored %d pressure %d\n",
-			dev_name(&ts->spi->dev), packet->tc.ignore, Rt);
-#endif
-		hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
-			      HRTIMER_MODE_REL);
+	* the maximum. Don't report it to user space, repeat at least
+	* once more the measurement */
+	if (ts->tc.ignore || Rt > ts->pressure_max) {
+		mod_timer(&ts->timer, jiffies + TS_POLL_PERIOD);
 		return;
 	}
 
-	/* Maybe check the pendown state before reporting. This discards
-	 * false readings when the pen is lifted.
+	/* NOTE:  "pendown" is inferred from pressure; we don't rely on
+	 * being able to check nPENIRQ status, or "friendly" trigger modes
+	 * (both-edges is much better than just-falling or low-level).
+	 *
+	 * REVISIT:  some boards may require reading nPENIRQ; it's
+	 * needed on 7843.  and 7845 reads pressure differently...
+	 *
+	 * REVISIT:  the touchscreen might not be connected; this code
+	 * won't notice that, even if nPENIRQ never fires ...
 	 */
-	if (ts->penirq_recheck_delay_usecs) {
-		udelay(ts->penirq_recheck_delay_usecs);
-		if (!get_pendown_state(ts))
-			Rt = 0;
+	if (!ts->pendown && Rt != 0) {
+		input_report_key(input_dev, BTN_TOUCH, 1);
+		sync = 1;
+	} else if (ts->pendown && Rt == 0) {
+		input_report_key(input_dev, BTN_TOUCH, 0);
+		sync = 1;
 	}
 
-	/* NOTE: We can't rely on the pressure to determine the pen down
-	 * state, even this controller has a pressure sensor.  The pressure
-	 * value can fluctuate for quite a while after lifting the pen and
-	 * in some cases may not even settle at the expected value.
-	 *
-	 * The only safe way to check for the pen up condition is in the
-	 * timer by reading the pen signal state (it's a GPIO _and_ IRQ).
-	 */
 	if (Rt) {
-		struct input_dev *input = ts->input;
+		input_report_abs(input_dev, ABS_X, x);
+		input_report_abs(input_dev, ABS_Y, y);
+		sync = 1;
+	}
 
-		if (!ts->pendown) {
-			input_report_key(input, BTN_TOUCH, 1);
-			ts->pendown = 1;
-#ifdef VERBOSE
-			dev_dbg(&ts->spi->dev, "DOWN\n");
+	if (sync) {
+		input_report_abs(input_dev, ABS_PRESSURE, Rt);
+		input_sync(input_dev);
+	}
+
+#ifdef	VERBOSE
+	if (Rt || ts->pendown)
+		pr_debug("%s: %d/%d/%d%s\n", dev_name(&ts->spi->dev),
+			x, y, Rt, Rt ? "" : " UP");
 #endif
+
+	spin_lock_irqsave(&ts->lock, flags);
+
+	ts->pendown = (Rt != 0);
+	mod_timer(&ts->timer, jiffies + TS_POLL_PERIOD);
+
+	spin_unlock_irqrestore(&ts->lock, flags);
+}
+
+static inline u16 ad7843_get_sample_val(u32 sample)
+{
+	return (((((sample & 0x00ff0000) >> 8) | (sample >> 24)) >> 3) & 0x0fff);
+}
+
+static u32 ad7843_get_better_values(struct ads7846 *ts, int index, int skiplimit)
+{
+	u32 diff12, diff23, diff31;
+	u32 vals[3];
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		vals[i] = ad7843_get_sample_val(ts->rxbuf[index+i]);
+		if (vals[i] == 0x0fff || vals[i] == 0) {
+			ts->skip_this_sample = 1;
+			return 0;
 		}
+	}
 
-		if (ts->swap_xy)
-			swap(x, y);
+	diff12 = (vals[0] > vals[1]) ? vals[0] - vals[1] : vals[1] - vals[0];
+	if (diff12 > skiplimit) {
+		ts->skip_this_sample = 1;
+		return 0;
+	}
 
-		input_report_abs(input, ABS_X, x);
-		input_report_abs(input, ABS_Y, y);
-		input_report_abs(input, ABS_PRESSURE, Rt);
+	diff23 = (vals[1] > vals[2]) ? vals[1] - vals[2] : vals[2] - vals[1];
+	if (diff23 > skiplimit) {
+		ts->skip_this_sample = 1;
+		return 0;
+	}
 
-		input_sync(input);
-#ifdef VERBOSE
-		dev_dbg(&ts->spi->dev, "%4d/%4d/%4d\n", x, y, Rt);
-#endif
+	diff31 = (vals[2] > vals[0]) ? vals[2] - vals[0] : vals[0] - vals[2];
+	if (diff31 > skiplimit) {
+		ts->skip_this_sample = 1;
+		return 0;
 	}
 
-	hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
-			HRTIMER_MODE_REL);
+	if (diff12 < diff23 && diff12 < diff31)
+		return (vals[0] + vals[1]) / 2;
+	if (diff23 < diff12 && diff23 < diff31)
+		return (vals[1] + vals[2]) / 2;
+
+	return (vals[0] + vals[2]) / 2;
 }
 
-static int ads7846_debounce(void *ads, int data_idx, int *val)
+static void ads7843_rx_average(void *ads)
+{
+	struct ads7846          *ts = ads;
+	struct input_dev        *input_dev = ts->input;
+	u16                     x, y, temp;
+	unsigned long           flags;
+	int i, sample_count;
+
+	dev_dbg(&ts->spi->dev, "%s\n", __FUNCTION__);
+
+	for (i = 0, y = 0, x = 0, sample_count = 0; i < (ts->buflen * 3 / 2); i+=3) {
+		if (i >= ts->skip_samples*3) {
+			temp = ad7843_get_better_values(ts, i, MAX_DIFF_BETWEEN_SAMPLES_Y);
+			if (!ts->skip_this_sample) {
+				if (ts->rotate == 180) {
+					y += MAX_12BIT - temp;
+				} else if (ts->rotate == 0) {
+					y += temp;
+				} else {
+					dev_info(&ts->spi->dev,
+						 "Rotate mode %d, not implemented yet\n",
+						 ts->rotate);
+				}
+				sample_count++;
+			}
+		}
+		ts->skip_this_sample = 0;
+	}
+
+	if (!sample_count)
+		goto sample_taken;
+
+	y /= sample_count;
+
+	for (sample_count = 0; i < (ts->buflen * 3); i+=3) {
+		if (i >= (ts->skip_samples + ts->buflen / 2)*3) {
+			temp = ad7843_get_better_values(ts, i, MAX_DIFF_BETWEEN_SAMPLES_X);
+			if (!ts->skip_this_sample) {
+				if (ts->rotate == 180) {
+					x += MAX_12BIT - temp;
+				} else if (ts->rotate == 0) {
+					x += temp;
+				} else {
+					dev_info(&ts->spi->dev,
+						 "Rotate mode %d, not implemented yet\n",
+						 ts->rotate);
+				}
+				sample_count++;
+			}
+		}
+		ts->skip_this_sample = 0;
+	}
+
+	if (!sample_count)
+		goto sample_taken;
+
+	x /= sample_count;
+
+       if (ts->pendown) {
+
+               input_report_key(input_dev, BTN_TOUCH, 1);
+               input_report_abs(input_dev, ABS_PRESSURE, ts->pressure_max / 2);
+               input_report_abs(input_dev, ABS_X, x);
+               input_report_abs(input_dev, ABS_Y, y);
+       } else {
+               input_report_key(input_dev, BTN_TOUCH, 0);
+               input_report_abs(input_dev, ABS_PRESSURE, 0);
+       }
+
+       input_sync(input_dev);
+       dev_dbg(&ts->spi->dev, "%d/%d %s\n", x, y, ts->pendown ? "" : " UP");
+
+sample_taken:
+       if (ts->pendown) {
+               spin_lock_irqsave(&ts->lock, flags);
+               mod_timer(&ts->timer, jiffies + TS_POLL_PERIOD);
+               spin_unlock_irqrestore(&ts->lock, flags);
+       }
+}
+
+static void ads7846_debounce(void *ads)
 {
 	struct ads7846		*ts = ads;
+	struct spi_message	*m;
+	struct spi_transfer	*t;
+	int			val;
+	int			status;
 
-	if (!ts->read_cnt || (abs(ts->last_read - *val) > ts->debounce_tol)) {
-		/* Start over collecting consistent readings. */
-		ts->read_rep = 0;
+	m = &ts->msg[ts->msg_idx];
+	t = list_entry(m->transfers.prev, struct spi_transfer, transfer_list);
+	val = (be16_to_cpu(*(__be16 *)t->rx_buf) >> 3) & 0x0fff;
+	if (!ts->read_cnt || (abs(ts->last_read - val) > ts->debounce_tol)) {
 		/* Repeat it, if this was the first read or the read
 		 * wasn't consistent enough. */
 		if (ts->read_cnt < ts->debounce_max) {
-			ts->last_read = *val;
+			ts->last_read = val;
 			ts->read_cnt++;
-			return ADS7846_FILTER_REPEAT;
 		} else {
 			/* Maximum number of debouncing reached and still
 			 * not enough number of consistent readings. Abort
 			 * the whole sample, repeat it in the next sampling
 			 * period.
 			 */
+			ts->tc.ignore = 1;
 			ts->read_cnt = 0;
-			return ADS7846_FILTER_IGNORE;
+			/* Last message will contain ads7846_rx() as the
+			 * completion function.
+			 */
+			m = ts->last_msg;
 		}
+		/* Start over collecting consistent readings. */
+		ts->read_rep = 0;
 	} else {
 		if (++ts->read_rep > ts->debounce_rep) {
 			/* Got a good reading for this coordinate,
 			 * go for the next one. */
+			ts->tc.ignore = 0;
+			ts->msg_idx++;
 			ts->read_cnt = 0;
 			ts->read_rep = 0;
-			return ADS7846_FILTER_OK;
-		} else {
+			m++;
+		} else
 			/* Read more values that are consistent. */
 			ts->read_cnt++;
-			return ADS7846_FILTER_REPEAT;
-		}
 	}
-}
-
-static int ads7846_no_filter(void *ads, int data_idx, int *val)
-{
-	return ADS7846_FILTER_OK;
-}
-
-static void ads7846_rx_val(void *ads)
-{
-	struct ads7846 *ts = ads;
-	struct ads7846_packet *packet = ts->packet;
-	struct spi_message *m;
-	struct spi_transfer *t;
-	int val;
-	int action;
-	int status;
-
-	m = &ts->msg[ts->msg_idx];
-	t = list_entry(m->transfers.prev, struct spi_transfer, transfer_list);
-
-	/* adjust:  on-wire is a must-ignore bit, a BE12 value, then padding;
-	 * built from two 8 bit values written msb-first.
-	 */
-	val = be16_to_cpup((__be16 *)t->rx_buf) >> 3;
-
-	action = ts->filter(ts->filter_data, ts->msg_idx, &val);
-	switch (action) {
-	case ADS7846_FILTER_REPEAT:
-		break;
-	case ADS7846_FILTER_IGNORE:
-		packet->tc.ignore = 1;
-		/* Last message will contain ads7846_rx() as the
-		 * completion function.
-		 */
-		m = ts->last_msg;
-		break;
-	case ADS7846_FILTER_OK:
-		*(u16 *)t->rx_buf = val;
-		packet->tc.ignore = 0;
-		m = &ts->msg[++ts->msg_idx];
-		break;
-	default:
-		BUG();
-	}
-	ts->wait_for_sync();
 	status = spi_async(ts->spi, m);
 	if (status)
 		dev_err(&ts->spi->dev, "spi_async --> %d\n",
 				status);
 }
 
-static enum hrtimer_restart ads7846_timer(struct hrtimer *handle)
+static void ads7846_timer(unsigned long handle)
 {
-	struct ads7846	*ts = container_of(handle, struct ads7846, timer);
-	int		status = 0;
+	struct ads7846		*ts = (void *)handle;
+	struct input_dev        *input_dev = ts->input;
+	int			status = 0;
 
-	spin_lock(&ts->lock);
-
-	if (unlikely(!get_pendown_state(ts) ||
-		     device_suspended(&ts->spi->dev))) {
-		if (ts->pendown) {
-			struct input_dev *input = ts->input;
-
-			input_report_key(input, BTN_TOUCH, 0);
-			input_report_abs(input, ABS_PRESSURE, 0);
-			input_sync(input);
+	/* get sample */
+	ts->pendown = ts->get_pendown_state();
+	spin_lock_irq(&ts->lock);
+	if (ts->model == 7843) {
+		ts->pending = 0;
+		if (unlikely(!ts->pendown)) {
 
-			ts->pendown = 0;
-#ifdef VERBOSE
-			dev_dbg(&ts->spi->dev, "UP\n");
-#endif
-		}
+			input_report_key(input_dev, BTN_TOUCH, 0);
+			input_report_abs(input_dev, ABS_PRESSURE, 0);
+			input_sync(input_dev);
 
-		/* measurement cycle ended */
-		if (!device_suspended(&ts->spi->dev)) {
-			ts->irq_disabled = 0;
-			enable_irq(ts->spi->irq);
+			if (!device_suspended(&ts->spi->dev)) {
+				ts->irq_disabled = 0;
+				enable_irq(ts->spi->irq);
+			}
+		} else {
+			/* pen is still down, continue with the measurement */
+			status = spi_async(ts->spi, &ts->msg[0]);
+			if (status)
+				dev_err(&ts->spi->dev, "spi_async --> %d\n", status);
 		}
-		ts->pending = 0;
 	} else {
-		/* pen is still down, continue with the measurement */
-		ts->msg_idx = 0;
-		ts->wait_for_sync();
-		status = spi_async(ts->spi, &ts->msg[0]);
-		if (status)
-			dev_err(&ts->spi->dev, "spi_async --> %d\n", status);
+		if (unlikely(ts->msg_idx && !ts->pendown)) {
+			/* measurement cycle ended */
+			if (!device_suspended(&ts->spi->dev)) {
+				ts->irq_disabled = 0;
+				enable_irq(ts->spi->irq);
+			}
+			ts->pending = 0;
+			ts->msg_idx = 0;
+		} else {
+			/* pen is still down, continue with the measurement */
+			ts->msg_idx = 0;
+			status = spi_async(ts->spi, &ts->msg[0]);
+			if (status)
+				dev_err(&ts->spi->dev, "spi_async --> %d\n", status);
+		}
 	}
-
-	spin_unlock(&ts->lock);
-	return HRTIMER_NORESTART;
+	spin_unlock_irq(&ts->lock);
 }
 
 static irqreturn_t ads7846_irq(int irq, void *handle)
@@ -753,7 +687,8 @@ static irqreturn_t ads7846_irq(int irq, void *handle)
 	unsigned long flags;
 
 	spin_lock_irqsave(&ts->lock, flags);
-	if (likely(get_pendown_state(ts))) {
+
+	if (likely(ts->get_pendown_state())) {
 		if (!ts->irq_disabled) {
 			/* The ARM do_simple_IRQ() dispatcher doesn't act
 			 * like the other dispatchers:  it will report IRQs
@@ -763,8 +698,7 @@ static irqreturn_t ads7846_irq(int irq, void *handle)
 			ts->irq_disabled = 1;
 			disable_irq_nosync(ts->spi->irq);
 			ts->pending = 1;
-			hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_DELAY),
-					HRTIMER_MODE_REL);
+			mod_timer(&ts->timer, jiffies);
 		}
 	}
 	spin_unlock_irqrestore(&ts->lock, flags);
@@ -785,7 +719,7 @@ static void ads7846_disable(struct ads7846 *ts)
 	/* are we waiting for IRQ, or polling? */
 	if (!ts->pending) {
 		ts->irq_disabled = 1;
-		disable_irq(ts->spi->irq);
+		disable_irq_nosync(ts->spi->irq);
 	} else {
 		/* the timer will run at least once more, and
 		 * leave everything in a clean state, IRQ disabled
@@ -800,6 +734,7 @@ static void ads7846_disable(struct ads7846 *ts)
 	/* we know the chip's in lowpower mode since we always
 	 * leave it that way after every request
 	 */
+
 }
 
 /* Must be called with ts->lock held */
@@ -819,7 +754,7 @@ static int ads7846_suspend(struct spi_device *spi, pm_message_t message)
 
 	spin_lock_irq(&ts->lock);
 
-	ts->is_suspended = 1;
+	spi->dev.power.power_state = message;
 	ads7846_disable(ts);
 
 	spin_unlock_irq(&ts->lock);
@@ -834,7 +769,7 @@ static int ads7846_resume(struct spi_device *spi)
 
 	spin_lock_irq(&ts->lock);
 
-	ts->is_suspended = 0;
+	spi->dev.power.power_state = PMSG_ON;
 	ads7846_enable(ts);
 
 	spin_unlock_irq(&ts->lock);
@@ -842,45 +777,13 @@ static int ads7846_resume(struct spi_device *spi)
 	return 0;
 }
 
-static int __devinit setup_pendown(struct spi_device *spi, struct ads7846 *ts)
-{
-	struct ads7846_platform_data *pdata = spi->dev.platform_data;
-	int err;
-
-	/* REVISIT when the irq can be triggered active-low, or if for some
-	 * reason the touchscreen isn't hooked up, we don't need to access
-	 * the pendown state.
-	 */
-	if (!pdata->get_pendown_state && !gpio_is_valid(pdata->gpio_pendown)) {
-		dev_err(&spi->dev, "no get_pendown_state nor gpio_pendown?\n");
-		return -EINVAL;
-	}
-
-	if (pdata->get_pendown_state) {
-		ts->get_pendown_state = pdata->get_pendown_state;
-		return 0;
-	}
-
-	err = gpio_request(pdata->gpio_pendown, "ads7846_pendown");
-	if (err) {
-		dev_err(&spi->dev, "failed to request pendown GPIO%d\n",
-				pdata->gpio_pendown);
-		return err;
-	}
-
-	ts->gpio_pendown = pdata->gpio_pendown;
-	return 0;
-}
-
 static int __devinit ads7846_probe(struct spi_device *spi)
 {
 	struct ads7846			*ts;
-	struct ads7846_packet		*packet;
 	struct input_dev		*input_dev;
 	struct ads7846_platform_data	*pdata = spi->dev.platform_data;
 	struct spi_message		*m;
 	struct spi_transfer		*x;
-	int				vref;
 	int				err;
 
 	if (!spi->irq) {
@@ -900,33 +803,46 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 		return -EINVAL;
 	}
 
+	/* REVISIT when the irq can be triggered active-low, or if for some
+	 * reason the touchscreen isn't hooked up, we don't need to access
+	 * the pendown state.
+	 */
+	if (pdata->get_pendown_state == NULL) {
+		dev_dbg(&spi->dev, "no get_pendown_state function?\n");
+		return -EINVAL;
+	}
+
 	/* We'd set TX wordsize 8 bits and RX wordsize to 13 bits ... except
 	 * that even if the hardware can do that, the SPI controller driver
 	 * may not.  So we stick to very-portable 8 bit words, both RX and TX.
 	 */
 	spi->bits_per_word = 8;
-	spi->mode = SPI_MODE_0;
-	err = spi_setup(spi);
-	if (err < 0)
-		return err;
 
 	ts = kzalloc(sizeof(struct ads7846), GFP_KERNEL);
-	packet = kzalloc(sizeof(struct ads7846_packet), GFP_KERNEL);
 	input_dev = input_allocate_device();
-	if (!ts || !packet || !input_dev) {
+	if (!ts || !input_dev) {
 		err = -ENOMEM;
 		goto err_free_mem;
 	}
 
 	dev_set_drvdata(&spi->dev, ts);
 
-	ts->packet = packet;
+	spi->dev.power.power_state = PMSG_ON;
+
 	ts->spi = spi;
 	ts->input = input_dev;
-	ts->vref_mv = pdata->vref_mv;
-	ts->swap_xy = pdata->swap_xy;
 
-	hrtimer_init(&ts->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	ts->buflen = pdata->buflen ? : MAX_BUF_SAMPLE_LEN;
+	ts->buflen = ts->buflen & ~0x1;	/* must be even */
+
+	if (ads7843_setup_buffers(&spi->dev)) {
+		dev_dbg(&spi->dev, "error allocating memory for sample buffers\n");
+		err = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	init_timer(&ts->timer);
+	ts->timer.data = (unsigned long) ts;
 	ts->timer.function = ads7846_timer;
 
 	spin_lock_init(&ts->lock);
@@ -935,40 +851,22 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 	ts->vref_delay_usecs = pdata->vref_delay_usecs ? : 100;
 	ts->x_plate_ohms = pdata->x_plate_ohms ? : 400;
 	ts->pressure_max = pdata->pressure_max ? : ~0;
+	ts->skip_samples = pdata->skip_samples ? : 0;
+	ts->rotate = pdata->rotate ? : 0;
 
-	if (pdata->filter != NULL) {
-		if (pdata->filter_init != NULL) {
-			err = pdata->filter_init(pdata, &ts->filter_data);
-			if (err < 0)
-				goto err_free_mem;
-		}
-		ts->filter = pdata->filter;
-		ts->filter_cleanup = pdata->filter_cleanup;
-	} else if (pdata->debounce_max) {
+	if (pdata->debounce_max) {
 		ts->debounce_max = pdata->debounce_max;
-		if (ts->debounce_max < 2)
-			ts->debounce_max = 2;
 		ts->debounce_tol = pdata->debounce_tol;
 		ts->debounce_rep = pdata->debounce_rep;
-		ts->filter = ads7846_debounce;
-		ts->filter_data = ts;
+		if (ts->debounce_rep > ts->debounce_max + 1)
+			ts->debounce_rep = ts->debounce_max - 1;
 	} else
-		ts->filter = ads7846_no_filter;
-
-	err = setup_pendown(spi, ts);
-	if (err)
-		goto err_cleanup_filter;
-
-	if (pdata->penirq_recheck_delay_usecs)
-		ts->penirq_recheck_delay_usecs =
-				pdata->penirq_recheck_delay_usecs;
-
-	ts->wait_for_sync = pdata->wait_for_sync ? : null_wait_for_sync;
+		ts->debounce_tol = ~0;
+	ts->get_pendown_state = pdata->get_pendown_state;
 
 	snprintf(ts->phys, sizeof(ts->phys), "%s/input0", dev_name(&spi->dev));
-	snprintf(ts->name, sizeof(ts->name), "ADS%d Touchscreen", ts->model);
 
-	input_dev->name = ts->name;
+	input_dev->name = "ADS784x Touchscreen";
 	input_dev->phys = ts->phys;
 	input_dev->dev.parent = &spi->dev;
 
@@ -983,9 +881,8 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 			pdata->y_max ? : MAX_12BIT,
 			0, 0);
 	input_set_abs_params(input_dev, ABS_PRESSURE,
-			pdata->pressure_min, pdata->pressure_max, 0, 0);
-
-	vref = pdata->keep_vref_on;
+			pdata->pressure_min ? : 0,
+			pdata->pressure_max ? : 1, 0, 0);
 
 	/* set up the transfers to read touchscreen state; this assumes we
 	 * use formula #2 for pressure, not #3.
@@ -995,209 +892,176 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 
 	spi_message_init(m);
 
-	/* y- still on; turn on only y+ (and ADC) */
-	packet->read_y = READ_Y(vref);
-	x->tx_buf = &packet->read_y;
-	x->len = 1;
-	spi_message_add_tail(x, m);
-
-	x++;
-	x->rx_buf = &packet->tc.y;
-	x->len = 2;
-	spi_message_add_tail(x, m);
-
-	/* the first sample after switching drivers can be low quality;
-	 * optionally discard it, using a second one after the signals
-	 * have had enough time to stabilize.
-	 */
-	if (pdata->settle_delay_usecs) {
-		x->delay_usecs = pdata->settle_delay_usecs;
-
-		x++;
-		x->tx_buf = &packet->read_y;
-		x->len = 1;
+	if (ts->model == 7843) {
+		x->tx_buf = ts->txbuf;
+		x->rx_buf = ts->rxbuf;
+		x->len = ts->buflen * sizeof(u32) * 3; /* For every sample we take 3 samples and choose the better 2 */
 		spi_message_add_tail(x, m);
 
-		x++;
-		x->rx_buf = &packet->tc.y;
-		x->len = 2;
-		spi_message_add_tail(x, m);
-	}
-
-	m->complete = ads7846_rx_val;
-	m->context = ts;
-
-	m++;
-	spi_message_init(m);
-
-	/* turn y- off, x+ on, then leave in lowpower */
-	x++;
-	packet->read_x = READ_X(vref);
-	x->tx_buf = &packet->read_x;
-	x->len = 1;
-	spi_message_add_tail(x, m);
-
-	x++;
-	x->rx_buf = &packet->tc.x;
-	x->len = 2;
-	spi_message_add_tail(x, m);
-
-	/* ... maybe discard first sample ... */
-	if (pdata->settle_delay_usecs) {
-		x->delay_usecs = pdata->settle_delay_usecs;
+		m->complete = ads7843_rx_average;
+		m->context = ts;
 
-		x++;
-		x->tx_buf = &packet->read_x;
+		ts->last_msg = m;
+	} else {
+		/* y- still on; turn on only y+ (and ADC) */
+		ts->read_y = READ_Y;
+		x->tx_buf = &ts->read_y;
 		x->len = 1;
 		spi_message_add_tail(x, m);
 
 		x++;
-		x->rx_buf = &packet->tc.x;
+		x->rx_buf = &ts->tc.y;
 		x->len = 2;
 		spi_message_add_tail(x, m);
-	}
 
-	m->complete = ads7846_rx_val;
-	m->context = ts;
+		m->complete = ads7846_debounce;
+		m->context = ts;
 
-	/* turn y+ off, x- on; we'll use formula #2 */
-	if (ts->model == 7846) {
 		m++;
 		spi_message_init(m);
 
+		/* turn y- off, x+ on, then leave in lowpower */
 		x++;
-		packet->read_z1 = READ_Z1(vref);
-		x->tx_buf = &packet->read_z1;
+		ts->read_x = READ_X;
+		x->tx_buf = &ts->read_x;
 		x->len = 1;
 		spi_message_add_tail(x, m);
 
 		x++;
-		x->rx_buf = &packet->tc.z1;
+		x->rx_buf = &ts->tc.x;
 		x->len = 2;
 		spi_message_add_tail(x, m);
 
-		/* ... maybe discard first sample ... */
-		if (pdata->settle_delay_usecs) {
-			x->delay_usecs = pdata->settle_delay_usecs;
+		m->complete = ads7846_debounce;
+		m->context = ts;
+
+		/* turn y+ off, x- on; we'll use formula #2 */
+		if (ts->model == 7846) {
+			m++;
+			spi_message_init(m);
 
 			x++;
-			x->tx_buf = &packet->read_z1;
+			ts->read_z1 = READ_Z1;
+			x->tx_buf = &ts->read_z1;
 			x->len = 1;
 			spi_message_add_tail(x, m);
 
 			x++;
-			x->rx_buf = &packet->tc.z1;
+			x->rx_buf = &ts->tc.z1;
 			x->len = 2;
 			spi_message_add_tail(x, m);
-		}
-
-		m->complete = ads7846_rx_val;
-		m->context = ts;
-
-		m++;
-		spi_message_init(m);
 
-		x++;
-		packet->read_z2 = READ_Z2(vref);
-		x->tx_buf = &packet->read_z2;
-		x->len = 1;
-		spi_message_add_tail(x, m);
-
-		x++;
-		x->rx_buf = &packet->tc.z2;
-		x->len = 2;
-		spi_message_add_tail(x, m);
+			m->complete = ads7846_debounce;
+			m->context = ts;
 
-		/* ... maybe discard first sample ... */
-		if (pdata->settle_delay_usecs) {
-			x->delay_usecs = pdata->settle_delay_usecs;
+			m++;
+			spi_message_init(m);
 
 			x++;
-			x->tx_buf = &packet->read_z2;
+			ts->read_z2 = READ_Z2;
+			x->tx_buf = &ts->read_z2;
 			x->len = 1;
 			spi_message_add_tail(x, m);
 
 			x++;
-			x->rx_buf = &packet->tc.z2;
+			x->rx_buf = &ts->tc.z2;
 			x->len = 2;
 			spi_message_add_tail(x, m);
-		}
 
-		m->complete = ads7846_rx_val;
-		m->context = ts;
-	}
+			m->complete = ads7846_debounce;
+			m->context = ts;
+		}
 
-	/* power down */
-	m++;
-	spi_message_init(m);
+		/* power down */
+		m++;
+		spi_message_init(m);
 
-	x++;
-	packet->pwrdown = PWRDOWN;
-	x->tx_buf = &packet->pwrdown;
-	x->len = 1;
-	spi_message_add_tail(x, m);
+		x++;
+		ts->pwrdown = PWRDOWN;
+		x->tx_buf = &ts->pwrdown;
+		x->len = 1;
+		spi_message_add_tail(x, m);
 
-	x++;
-	x->rx_buf = &packet->dummy;
-	x->len = 2;
-	CS_CHANGE(*x);
-	spi_message_add_tail(x, m);
+		x++;
+		x->rx_buf = &ts->dummy;
+		x->len = 2;
+		CS_CHANGE(*x);
+		spi_message_add_tail(x, m);
 
-	m->complete = ads7846_rx;
-	m->context = ts;
+		m->complete = ads7846_rx;
+		m->context = ts;
 
-	ts->last_msg = m;
+		ts->last_msg = m;
+	}
 
 	if (request_irq(spi->irq, ads7846_irq, IRQF_TRIGGER_FALLING,
 			spi->dev.driver->name, ts)) {
-		dev_info(&spi->dev,
-			"trying pin change workaround on irq %d\n", spi->irq);
-		err = request_irq(spi->irq, ads7846_irq,
-				  IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
-				  spi->dev.driver->name, ts);
-		if (err) {
-			dev_dbg(&spi->dev, "irq %d busy?\n", spi->irq);
-			goto err_free_gpio;
-		}
+		dev_dbg(&spi->dev, "irq %d busy?\n", spi->irq);
+		err = -EBUSY;
+		goto err_free_buf;
 	}
 
-	err = ads784x_hwmon_register(spi, ts);
-	if (err)
-		goto err_free_irq;
-
 	dev_info(&spi->dev, "touchscreen, irq %d\n", spi->irq);
 
-	/* take a first sample, leaving nPENIRQ active and vREF off; avoid
+	/* take a first sample, leaving nPENIRQ active; avoid
 	 * the touchscreen, in case it's not connected.
 	 */
-	(void) ads7846_read12_ser(&spi->dev,
-			  READ_12BIT_SER(vaux) | ADS_PD10_ALL_ON);
+	if (ts->model != 7843) {
+		/* take a first sample, leaving nPENIRQ active; avoid
+		 * the touchscreen, in case it's not connected.
+		 */
+		(void) ads7846_read12_ser(&spi->dev,
+				READ_12BIT_SER(vaux) | ADS_PD10_ALL_ON);
+	}
 
-	err = sysfs_create_group(&spi->dev.kobj, &ads784x_attr_group);
-	if (err)
-		goto err_remove_hwmon;
+	/* ads7843/7845 don't have temperature sensors, and
+	 * use the other sensors a bit differently too
+	 */
+	if (ts->model == 7846) {
+		device_create_file(&spi->dev, &dev_attr_temp0);
+		device_create_file(&spi->dev, &dev_attr_temp1);
+	}
+
+	if (ts->model != 7845 && ts->model != 7843)
+		device_create_file(&spi->dev, &dev_attr_vbatt);
+
+	if (ts->model != 7843) {
+		device_create_file(&spi->dev, &dev_attr_vaux);
+	}
+
+	device_create_file(&spi->dev, &dev_attr_pen_down);
+	device_create_file(&spi->dev, &dev_attr_disable);
 
 	err = input_register_device(input_dev);
 	if (err)
-		goto err_remove_attr_group;
+		goto err_remove_attr;
 
 	return 0;
 
- err_remove_attr_group:
-	sysfs_remove_group(&spi->dev.kobj, &ads784x_attr_group);
- err_remove_hwmon:
-	ads784x_hwmon_unregister(spi, ts);
- err_free_irq:
+ err_remove_attr:
+	device_remove_file(&spi->dev, &dev_attr_disable);
+	device_remove_file(&spi->dev, &dev_attr_pen_down);
+	if (ts->model == 7846) {
+		device_remove_file(&spi->dev, &dev_attr_temp1);
+		device_remove_file(&spi->dev, &dev_attr_temp0);
+	}
+
+	if (ts->model != 7845 && ts->model != 7843)
+		device_remove_file(&spi->dev, &dev_attr_vbatt);
+
+	if (ts->model != 7843) {
+		device_remove_file(&spi->dev, &dev_attr_vaux);
+	}
+
 	free_irq(spi->irq, ts);
- err_free_gpio:
-	if (ts->gpio_pendown != -1)
-		gpio_free(ts->gpio_pendown);
- err_cleanup_filter:
-	if (ts->filter_cleanup)
-		ts->filter_cleanup(ts->filter_data);
+
+ err_free_buf:
+	if (ts->txbuf)
+		kfree(ts->txbuf);
+	if (ts->rxbuf)
+		kfree(ts->rxbuf);
  err_free_mem:
 	input_free_device(input_dev);
-	kfree(packet);
 	kfree(ts);
 	return err;
 }
@@ -1206,24 +1070,33 @@ static int __devexit ads7846_remove(struct spi_device *spi)
 {
 	struct ads7846		*ts = dev_get_drvdata(&spi->dev);
 
-	ads784x_hwmon_unregister(spi, ts);
 	input_unregister_device(ts->input);
 
 	ads7846_suspend(spi, PMSG_SUSPEND);
 
-	sysfs_remove_group(&spi->dev.kobj, &ads784x_attr_group);
+	if (ts->txbuf)
+		kfree(ts->txbuf);
+	if (ts->rxbuf)
+		kfree(ts->rxbuf);
 
-	free_irq(ts->spi->irq, ts);
-	/* suspend left the IRQ disabled */
-	enable_irq(ts->spi->irq);
+	device_remove_file(&spi->dev, &dev_attr_disable);
+	device_remove_file(&spi->dev, &dev_attr_pen_down);
+	if (ts->model == 7846) {
+		device_remove_file(&spi->dev, &dev_attr_temp1);
+		device_remove_file(&spi->dev, &dev_attr_temp0);
+	}
+
+	if (ts->model != 7845 && ts->model != 7843)
+		device_remove_file(&spi->dev, &dev_attr_vbatt);
 
-	if (ts->gpio_pendown != -1)
-		gpio_free(ts->gpio_pendown);
+	if (ts->model != 7843) {
+		device_remove_file(&spi->dev, &dev_attr_vaux);
+	}
 
-	if (ts->filter_cleanup)
-		ts->filter_cleanup(ts->filter_data);
+	free_irq(ts->spi->irq, ts);
+	/* suspend left the IRQ disabled */
+	disable_irq_nosync(ts->spi->irq);
 
-	kfree(ts->packet);
 	kfree(ts);
 
 	dev_dbg(&spi->dev, "unregistered touchscreen\n");
diff --git a/drivers/input/touchscreen/mxc_ts.c b/drivers/input/touchscreen/mxc_ts.c
index 610f31e65778..6cce76f1494f 100644
--- a/drivers/input/touchscreen/mxc_ts.c
+++ b/drivers/input/touchscreen/mxc_ts.c
@@ -43,6 +43,9 @@
 #include <linux/pmic_external.h>
 #include <linux/pmic_adc.h>
 #include <linux/kthread.h>
+#ifdef CONFIG_EARLYSUSPEND
+#include <linux/earlysuspend.h>
+#endif
 
 #define MXC_TS_NAME	"mxc_ts"
 
@@ -60,6 +63,30 @@ static struct task_struct *tstask;
 static int calibration[7];
 module_param_array(calibration, int, NULL, S_IRUGO | S_IWUSR);
 
+#ifdef CONFIG_EARLYSUSPEND
+
+static wait_queue_head_t ts_wait;
+static int ts_suspend;
+
+static void stop_ts_early_suspend(struct early_suspend *h)
+{
+	ts_suspend = 1;
+}
+
+static void start_ts_late_resume(struct early_suspend *h)
+{
+	ts_suspend = 0;
+	wake_up_interruptible(&ts_wait);
+}
+
+static struct early_suspend stop_ts_early_suspend_desc = {
+	.level = EARLY_SUSPEND_LEVEL_STOP_DRAWING,
+	.suspend = stop_ts_early_suspend,
+	.resume = start_ts_late_resume,
+};
+
+#endif
+
 static int ts_thread(void *arg)
 {
 	t_touch_screen ts_sample;
@@ -69,6 +96,9 @@ static int ts_thread(void *arg)
 		int x, y;
 		static int last_x = -1, last_y = -1, last_press = -1;
 
+#ifdef CONFIG_EARLYSUSPEND
+		wait_event_interruptible(ts_wait, !ts_suspend);
+#endif
 		memset(&ts_sample, 0, sizeof(t_touch_screen));
 
 		/* After 2 consecutive samples with the pen up, enable irq waiting */
@@ -76,14 +106,13 @@ static int ts_thread(void *arg)
 			msleep(20);
 			continue;
 		}
-		if (!(ts_sample.contact_resistance || wait))
-		{
+		if (!(ts_sample.contact_resistance || wait)) {
 			msleep(20);
 			continue;
 		}
 
 		if (ts_sample.x_position == 0 && ts_sample.y_position == 0 &&
-			ts_sample.contact_resistance == 0) {
+		    ts_sample.contact_resistance == 0) {
 			x = last_x;
 			y = last_y;
 		} else if (calibration[6] == 0) {
@@ -91,14 +120,14 @@ static int ts_thread(void *arg)
 			y = ts_sample.y_position;
 		} else {
 			x = calibration[0] * (int)ts_sample.x_position +
-				calibration[1] * (int)ts_sample.y_position +
-				calibration[2];
+			    calibration[1] * (int)ts_sample.y_position +
+			    calibration[2];
 			x /= calibration[6];
 			if (x < 0)
 				x = 0;
 			y = calibration[3] * (int)ts_sample.x_position +
-				calibration[4] * (int)ts_sample.y_position +
-				calibration[5];
+			    calibration[4] * (int)ts_sample.y_position +
+			    calibration[5];
 			y /= calibration[6];
 			if (y < 0)
 				y = 0;
@@ -127,7 +156,7 @@ static int ts_thread(void *arg)
 		/* report the BTN_TOUCH */
 		if (ts_sample.contact_resistance != last_press)
 			input_event(mxc_inputdev, EV_KEY,
-					BTN_TOUCH, ts_sample.contact_resistance);
+				    BTN_TOUCH, ts_sample.contact_resistance);
 
 		input_sync(mxc_inputdev);
 		last_press = ts_sample.contact_resistance;
@@ -148,8 +177,7 @@ static int __init mxc_ts_init(void)
 
 	mxc_inputdev = input_allocate_device();
 	if (!mxc_inputdev) {
-		printk(KERN_ERR
-		       "mxc_ts_init: not enough memory\n");
+		printk(KERN_ERR "mxc_ts_init: not enough memory\n");
 		return -ENOMEM;
 	}
 
@@ -166,11 +194,14 @@ static int __init mxc_ts_init(void)
 
 	tstask = kthread_run(ts_thread, NULL, "mxc_ts");
 	if (IS_ERR(tstask)) {
-		printk(KERN_ERR
-			"mxc_ts_init: failed to create kthread");
+		printk(KERN_ERR "mxc_ts_init: failed to create kthread");
 		tstask = NULL;
 		return -1;
 	}
+#ifdef CONFIG_EARLYSUSPEND
+	init_waitqueue_head(&ts_wait);
+	register_early_suspend(&stop_ts_early_suspend_desc);
+#endif
 	printk("mxc input touchscreen loaded\n");
 	return 0;
 }
@@ -186,6 +217,9 @@ static void __exit mxc_ts_exit(void)
 		input_free_device(mxc_inputdev);
 		mxc_inputdev = NULL;
 	}
+#ifdef CONFIG_EARLYSUSPEND
+	unregister_early_suspend(&stop_ts_early_suspend_desc);
+#endif
 }
 
 late_initcall(mxc_ts_init);
diff --git a/drivers/leds/leds-mxs-pwm.c b/drivers/leds/leds-mxs-pwm.c
index c76821770446..a546900a44d0 100644
--- a/drivers/leds/leds-mxs-pwm.c
+++ b/drivers/leds/leds-mxs-pwm.c
@@ -165,10 +165,35 @@ static int __devexit mxs_pwm_led_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int mxs_led_suspend(struct platform_device *dev, pm_message_t state)
+{
+	int i;
+
+	for (i = 0; i < leds.led_num; i++)
+		led_classdev_suspend(&leds.leds[i].dev);
+	return 0;
+}
+
+static int mxs_led_resume(struct platform_device *dev)
+{
+	int i;
+
+	for (i = 0; i < leds.led_num; i++)
+		led_classdev_resume(&leds.leds[i].dev);
+	return 0;
+}
+#else
+#define mxs_led_suspend NULL
+#define mxs_led_resume NULL
+#endif
+
 
 static struct platform_driver mxs_pwm_led_driver = {
 	.probe   = mxs_pwm_led_probe,
 	.remove  = __devexit_p(mxs_pwm_led_remove),
+	.suspend = mxs_led_suspend,
+	.resume = mxs_led_resume,
 	.driver  = {
 		.name = "mxs-leds",
 	},
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index c64c8d201262..e7185d1cf281 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -592,6 +592,17 @@ config VIDEO_MXS_PXP
 	  To compile this driver as a module, choose M here: the
 	  module will be called pxp.
 
+config VIDEO_MXC_PXP_V4L2
+	tristate "MXC PxP V4L2 driver"
+	depends on VIDEO_DEV && VIDEO_V4L2 && ARCH_MX5
+	select VIDEOBUF_DMA_CONTIG
+	---help---
+	  This is a video4linux driver for the Freescale PxP
+	  (Pixel Pipeline). This module supports output overlay of
+	  the MXC framebuffer on a video stream.
+
+	  To compile this driver as a module, choose M here.
+
 config VIDEO_MXC_OPL
 	tristate
 	depends on VIDEO_DEV && ARCH_MXC
diff --git a/drivers/media/video/Makefile b/drivers/media/video/Makefile
index 49cb18802024..a4de2a151253 100644
--- a/drivers/media/video/Makefile
+++ b/drivers/media/video/Makefile
@@ -94,6 +94,7 @@ obj-$(CONFIG_VIDEO_MXC_CSI_CAMERA) += mxc/capture/
 obj-$(CONFIG_VIDEO_MXC_IPU_OUTPUT) += mxc/output/
 obj-$(CONFIG_VIDEO_MXC_IPUV1_WVGA_OUTPUT) += mxc/output/
 obj-$(CONFIG_VIDEO_MXC_EMMA_OUTPUT) += mxc/output/
+obj-$(CONFIG_VIDEO_MXC_PXP_V4L2) += mxc/output/
 obj-$(CONFIG_VIDEO_MXC_OPL) += mxc/opl/
 obj-$(CONFIG_VIDEO_PXP)	+= pxp.o
 obj-$(CONFIG_VIDEO_MXS_PXP)	+= mxs_pxp.o
diff --git a/drivers/media/video/mxc/capture/Kconfig b/drivers/media/video/mxc/capture/Kconfig
index adab0a886f36..276dfa424feb 100644
--- a/drivers/media/video/mxc/capture/Kconfig
+++ b/drivers/media/video/mxc/capture/Kconfig
@@ -58,6 +58,22 @@ config MXC_CAMERA_MICRON111
 	---help---
 	  If you plan to use the mt9v111 Camera with your MXC system, say Y here.
 
+config MXC_CAMERA_MICRON111_1
+    tristate "Micron mt9v111 camera 1 support"
+    select I2C_MXC
+    depends on ! VIDEO_MXC_EMMA_CAMERA
+    depends on MXC_CAMERA_MICRON111
+    ---help---
+      If you plan to use the mt9v111 Camera 1 with your MXC system, say Y here.
+
+config MXC_CAMERA_MICRON111_2
+    tristate "Micron mt9v111 camera 2 support"
+    select I2C_MXC
+    depends on ! VIDEO_MXC_EMMA_CAMERA
+    depends on MXC_CAMERA_MICRON111
+    ---help---
+      If you plan to use the mt9v111 Camera 2 with your MXC system, say Y here.
+
 config MXC_CAMERA_OV2640
 	tristate "OmniVision ov2640 camera support"
 	depends on !VIDEO_MXC_EMMA_CAMERA
@@ -72,7 +88,7 @@ config MXC_CAMERA_OV3640
 
 config MXC_TVIN_ADV7180
 	tristate "Analog Device adv7180 TV Decoder Input support"
-	depends on MACH_MX35_3DS
+	depends on (MACH_MX35_3DS || MACH_MX51_3DS)
 	---help---
 	  If you plan to use the adv7180 video decoder with your MXC system, say Y here.
 
diff --git a/drivers/media/video/mxc/capture/Makefile b/drivers/media/video/mxc/capture/Makefile
index 112923c8fc8f..03ff094171bf 100644
--- a/drivers/media/video/mxc/capture/Makefile
+++ b/drivers/media/video/mxc/capture/Makefile
@@ -35,5 +35,5 @@ obj-$(CONFIG_MXC_CAMERA_OV2640) += ov2640_camera.o
 ov3640_camera-objs := ov3640.o sensor_clock.o
 obj-$(CONFIG_MXC_CAMERA_OV3640) += ov3640_camera.o
 
-adv7180_tvin-objs := adv7180.o sensor_clock.o
+adv7180_tvin-objs := adv7180.o
 obj-$(CONFIG_MXC_TVIN_ADV7180) += adv7180_tvin.o
diff --git a/drivers/media/video/mxc/capture/adv7180.c b/drivers/media/video/mxc/capture/adv7180.c
index 1edee763bebc..527a0d1ad9fa 100644
--- a/drivers/media/video/mxc/capture/adv7180.c
+++ b/drivers/media/video/mxc/capture/adv7180.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2005-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -39,6 +39,7 @@ static struct regulator *dvddio_regulator;
 static struct regulator *dvdd_regulator;
 static struct regulator *avdd_regulator;
 static struct regulator *pvdd_regulator;
+static struct mxc_tvin_platform_data *tvin_plat;
 
 extern void gpio_sensor_active(void);
 extern void gpio_sensor_inactive(void);
@@ -118,26 +119,26 @@ static video_fmt_t video_fmts[] = {
 	{			/*! NTSC */
 	 .v4l2_id = V4L2_STD_NTSC,
 	 .name = "NTSC",
-	 .raw_width = 720 - 1,	/* SENS_FRM_WIDTH */
-	 .raw_height = 288 - 1,	/* SENS_FRM_HEIGHT */
+	 .raw_width = 720,	/* SENS_FRM_WIDTH */
+	 .raw_height = 525,	/* SENS_FRM_HEIGHT */
 	 .active_width = 720,	/* ACT_FRM_WIDTH plus 1 */
-	 .active_height = (480 / 2),	/* ACT_FRM_WIDTH plus 1 */
+	 .active_height = 480,	/* ACT_FRM_WIDTH plus 1 */
 	 },
 	{			/*! (B, G, H, I, N) PAL */
 	 .v4l2_id = V4L2_STD_PAL,
 	 .name = "PAL",
-	 .raw_width = 720 - 1,
-	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .raw_width = 720,
+	 .raw_height = 625,
 	 .active_width = 720,
-	 .active_height = (576 / 2),
+	 .active_height = 576,
 	 },
 	{			/*! Unlocked standard */
 	 .v4l2_id = V4L2_STD_ALL,
 	 .name = "Autodetect",
-	 .raw_width = 720 - 1,
-	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .raw_width = 720,
+	 .raw_height = 625,
 	 .active_width = 720,
-	 .active_height = (576 / 2),
+	 .active_height = 576,
 	 },
 };
 
@@ -246,6 +247,10 @@ static void adv7180_get_std(v4l2_std_id *std)
 
 	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_get_std\n");
 
+	/* Make sure power on */
+	if (tvin_plat->pwdn)
+		tvin_plat->pwdn(0);
+
 	/* Read the AD_RESULT to get the detect output video standard */
 	tmp = adv7180_read(ADV7180_STATUS_1) & 0x70;
 
@@ -335,6 +340,11 @@ static int ioctl_s_power(struct v4l2_int_device *s, int on)
 
 	if (on && !sensor->on) {
 		gpio_sensor_active();
+
+		/* Make sure pwoer on */
+		if (tvin_plat->pwdn)
+			tvin_plat->pwdn(0);
+
 		if (adv7180_write_reg(ADV7180_PWR_MNG, 0) != 0)
 			return -EIO;
 	} else if (!on && sensor->on) {
@@ -500,6 +510,10 @@ static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 
 	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_g_ctrl\n");
 
+	/* Make sure power on */
+	if (tvin_plat->pwdn)
+		tvin_plat->pwdn(0);
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		dev_dbg(&adv7180_data.i2c_client->dev,
@@ -593,6 +607,10 @@ static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 
 	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180:ioctl_s_ctrl\n");
 
+	/* Make sure power on */
+	if (tvin_plat->pwdn)
+		tvin_plat->pwdn(0);
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		dev_dbg(&adv7180_data.i2c_client->dev,
@@ -803,13 +821,13 @@ static int adv7180_probe(struct i2c_client *client,
 {
 	int rev_id;
 	int ret = 0;
-	struct mxc_tvin_platform_data *plat_data = client->dev.platform_data;
+	tvin_plat = client->dev.platform_data;
 
 	dev_dbg(&adv7180_data.i2c_client->dev, "In adv7180_probe\n");
 
-	if (plat_data->dvddio_reg) {
+	if (tvin_plat->dvddio_reg) {
 		dvddio_regulator =
-		    regulator_get(&client->dev, plat_data->dvddio_reg);
+		    regulator_get(&client->dev, tvin_plat->dvddio_reg);
 		if (!IS_ERR_VALUE((unsigned long)dvddio_regulator)) {
 			regulator_set_voltage(dvddio_regulator, 3300000, 3300000);
 			if (regulator_enable(dvddio_regulator) != 0)
@@ -817,9 +835,9 @@ static int adv7180_probe(struct i2c_client *client,
 		}
 	}
 
-	if (plat_data->dvdd_reg) {
+	if (tvin_plat->dvdd_reg) {
 		dvdd_regulator =
-		    regulator_get(&client->dev, plat_data->dvdd_reg);
+		    regulator_get(&client->dev, tvin_plat->dvdd_reg);
 		if (!IS_ERR_VALUE((unsigned long)dvdd_regulator)) {
 			regulator_set_voltage(dvdd_regulator, 1800000, 1800000);
 			if (regulator_enable(dvdd_regulator) != 0)
@@ -827,9 +845,9 @@ static int adv7180_probe(struct i2c_client *client,
 		}
 	}
 
-	if (plat_data->avdd_reg) {
+	if (tvin_plat->avdd_reg) {
 		avdd_regulator =
-		    regulator_get(&client->dev, plat_data->avdd_reg);
+		    regulator_get(&client->dev, tvin_plat->avdd_reg);
 		if (!IS_ERR_VALUE((unsigned long)avdd_regulator)) {
 			regulator_set_voltage(avdd_regulator, 1800000, 1800000);
 			if (regulator_enable(avdd_regulator) != 0)
@@ -837,9 +855,9 @@ static int adv7180_probe(struct i2c_client *client,
 		}
 	}
 
-	if (plat_data->pvdd_reg) {
+	if (tvin_plat->pvdd_reg) {
 		pvdd_regulator =
-		    regulator_get(&client->dev, plat_data->pvdd_reg);
+		    regulator_get(&client->dev, tvin_plat->pvdd_reg);
 		if (!IS_ERR_VALUE((unsigned long)pvdd_regulator)) {
 			regulator_set_voltage(pvdd_regulator, 1800000, 1800000);
 			if (regulator_enable(pvdd_regulator) != 0)
@@ -847,11 +865,12 @@ static int adv7180_probe(struct i2c_client *client,
 		}
 	}
 
-	if (plat_data->reset)
-		plat_data->reset();
 
-	if (plat_data->pwdn)
-		plat_data->pwdn(1);
+	if (tvin_plat->reset)
+		tvin_plat->reset();
+
+	if (tvin_plat->pwdn)
+		tvin_plat->pwdn(0);
 
 	msleep(1);
 
@@ -913,7 +932,7 @@ static int adv7180_detach(struct i2c_client *client)
 		__func__, IF_NAME, client->addr << 1, client->adapter->name);
 
 	if (plat_data->pwdn)
-		plat_data->pwdn(0);
+		plat_data->pwdn(1);
 
 	if (dvddio_regulator) {
 		regulator_disable(dvddio_regulator);
diff --git a/drivers/media/video/mxc/capture/csi_v4l2_capture.c b/drivers/media/video/mxc/capture/csi_v4l2_capture.c
index 9bddc3692996..cf224e0673f0 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 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -789,15 +789,15 @@ static ssize_t csi_v4l_read(struct file *file, char *buf, size_t count,
 							  (cam->v2f.fmt.
 							   pix.sizeimage),
 							  &cam->
-							  still_buf,
+							  still_buf[0],
 							  GFP_DMA | GFP_KERNEL);
 		if (cam->still_buf_vaddr == NULL) {
 			pr_err("alloc dma memory failed\n");
 			return -ENOMEM;
 		}
 		cam->still_counter = 0;
-		__raw_writel(cam->still_buf, CSI_CSIDMASA_FB2);
-		__raw_writel(cam->still_buf, CSI_CSIDMASA_FB1);
+		__raw_writel(cam->still_buf[0], CSI_CSIDMASA_FB2);
+		__raw_writel(cam->still_buf[0], CSI_CSIDMASA_FB1);
 		__raw_writel(__raw_readl(CSI_CSICR3) | BIT_DMA_REFLASH_RFF,
 			     CSI_CSICR3);
 		__raw_writel(__raw_readl(CSI_CSISR), CSI_CSISR);
@@ -813,8 +813,8 @@ static ssize_t csi_v4l_read(struct file *file, char *buf, size_t count,
 
 	if (cam->still_buf_vaddr != NULL) {
 		dma_free_coherent(0, PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
-				  cam->still_buf_vaddr, cam->still_buf);
-		cam->still_buf = 0;
+				  cam->still_buf_vaddr, cam->still_buf[0]);
+		cam->still_buf[0] = 0;
 		cam->still_buf_vaddr = NULL;
 	}
 
diff --git a/drivers/media/video/mxc/capture/emma_v4l2_capture.c b/drivers/media/video/mxc/capture/emma_v4l2_capture.c
index 9cb08b26f1cd..170807716ec6 100644
--- a/drivers/media/video/mxc/capture/emma_v4l2_capture.c
+++ b/drivers/media/video/mxc/capture/emma_v4l2_capture.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -858,7 +858,7 @@ static void mxc_csi_dma_chaining(void *data)
 
 		/* Config DMA */
 		memset(&dma_request, 0, sizeof(mxc_dma_requestbuf_t));
-		dma_request.dst_addr = cam->still_buf
+		dma_request.dst_addr = cam->still_buf[0]
 		    + (chained % max_dma) * CSI_DMA_LENGTH;
 		dma_request.src_addr = (dma_addr_t) CSI_CSIRXFIFO_PHYADDR;
 		dma_request.num_of_bytes = count;
@@ -1040,7 +1040,7 @@ mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
 
 	cam->still_buf_vaddr = dma_alloc_coherent(0,
 						  PAGE_ALIGN(CSI_MEM_SIZE),
-						  &cam->still_buf,
+						  &cam->still_buf[0],
 						  GFP_DMA | GFP_KERNEL);
 
 	if (!cam->still_buf_vaddr) {
@@ -1120,8 +1120,8 @@ mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
 
       exit1:
 	dma_free_coherent(0, PAGE_ALIGN(CSI_MEM_SIZE),
-			  cam->still_buf_vaddr, cam->still_buf);
-	cam->still_buf = 0;
+			  cam->still_buf_vaddr, cam->still_buf[0]);
+	cam->still_buf[0] = 0;
 
       exit0:
 	up(&cam->busy_lock);
@@ -1160,7 +1160,8 @@ mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
 
 	v_address = dma_alloc_coherent(0,
 				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
-				       &cam->still_buf, GFP_DMA | GFP_KERNEL);
+				       &cam->still_buf[0],
+				       GFP_DMA | GFP_KERNEL);
 
 	if (!v_address) {
 		pr_info("mxc_v4l_read failed at allocate still_buf\n");
@@ -1194,8 +1195,8 @@ mxc_v4l_read(struct file *file, char *buf, size_t count, loff_t *ppos)
 
       exit1:
 	dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address,
-			  cam->still_buf);
-	cam->still_buf = 0;
+			  cam->still_buf[0]);
+	cam->still_buf[0] = 0;
 
       exit0:
 	up(&cam->busy_lock);
diff --git a/drivers/media/video/mxc/capture/ipu_csi_enc.c b/drivers/media/video/mxc/capture/ipu_csi_enc.c
index fd3f0c132c14..0b87282551ff 100644
--- a/drivers/media/video/mxc/capture/ipu_csi_enc.c
+++ b/drivers/media/video/mxc/capture/ipu_csi_enc.c
@@ -25,6 +25,7 @@
 #include "ipu_prp_sw.h"
 
 #ifdef CAMERA_DBG
+	extern void ipu_dump_registers(void);
 	#define CAMERA_TRACE(x) (printk)x
 #else
 	#define CAMERA_TRACE(x)
@@ -66,6 +67,7 @@ static int csi_enc_setup(cam_data *cam)
 	u32 pixel_fmt;
 	int err = 0;
 	dma_addr_t dummy = cam->dummy_frame.buffer.m.offset;
+	ipu_channel_t channel;
 
 	CAMERA_TRACE("In csi_enc_setup\n");
 	if (!cam) {
@@ -101,13 +103,18 @@ static int csi_enc_setup(cam_data *cam)
 
 	ipu_csi_enable_mclk_if(CSI_MCLK_ENC, cam->csi, true, true);
 
-	err = ipu_init_channel(CSI_MEM, &params);
+	if (cam->csi == 0)
+		channel = CSI_MEM0;
+	else
+		channel = CSI_MEM1;
+
+	err = ipu_init_channel(channel, &params);
 	if (err != 0) {
 		printk(KERN_ERR "ipu_init_channel %d\n", err);
 		return err;
 	}
 
-	err = ipu_init_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+	err = ipu_init_channel_buffer(channel, IPU_OUTPUT_BUFFER,
 				      pixel_fmt, cam->v2f.fmt.pix.width,
 				      cam->v2f.fmt.pix.height,
 				      cam->v2f.fmt.pix.width, IPU_ROTATE_NONE,
@@ -115,12 +122,12 @@ static int csi_enc_setup(cam_data *cam)
 				      cam->offset.u_offset,
 				      cam->offset.v_offset);
 	if (err != 0) {
-		printk(KERN_ERR "CSI_MEM output buffer\n");
+		printk(KERN_ERR "CSI_MEM%d output buffer\n",cam->csi);
 		return err;
 	}
-	err = ipu_enable_channel(CSI_MEM);
+	err = ipu_enable_channel(channel);
 	if (err < 0) {
-		printk(KERN_ERR "ipu_enable_channel CSI_MEM\n");
+		printk(KERN_ERR "ipu_enable_channel CSI_MEM%d\n",cam->csi);
 		return err;
 	}
 
@@ -135,24 +142,34 @@ static int csi_enc_setup(cam_data *cam)
  *
  * @return  status
  */
-static int csi_enc_eba_update(dma_addr_t eba, int *buffer_num)
+static int csi_enc_eba_update(int csi, dma_addr_t eba, int *buffer_num)
 {
 	int err = 0;
+	ipu_channel_t channel;
+
+	if (csi == 0)
+		channel = CSI_MEM0;
+	else
+		channel = CSI_MEM1;
 
-	pr_debug("eba %x\n", eba);
-	err = ipu_update_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+	err = ipu_update_channel_buffer(channel, IPU_OUTPUT_BUFFER,
 					*buffer_num, eba);
+
 	if (err != 0) {
-		ipu_clear_buffer_ready(CSI_MEM, IPU_OUTPUT_BUFFER,
+		ipu_clear_buffer_ready(channel, IPU_OUTPUT_BUFFER,
 				       *buffer_num);
 		printk(KERN_ERR "err %d buffer_num %d\n", err, *buffer_num);
 		return err;
 	}
 
-	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, *buffer_num);
+	ipu_select_buffer(channel, IPU_OUTPUT_BUFFER, *buffer_num);
 
 	*buffer_num = (*buffer_num == 0) ? 1 : 0;
 
+#ifdef CAMERA_DBG
+	ipu_dump_registers ();
+#endif
+
 	return 0;
 }
 
@@ -166,6 +183,7 @@ static int csi_enc_enabling_tasks(void *private)
 {
 	cam_data *cam = (cam_data *) private;
 	int err = 0;
+	int ipu_irq_csi_out_eof;
 	CAMERA_TRACE("IPU:In csi_enc_enabling_tasks\n");
 
 	cam->dummy_frame.vaddress = dma_alloc_coherent(0,
@@ -182,11 +200,16 @@ static int csi_enc_enabling_tasks(void *private)
 	    PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage);
 	cam->dummy_frame.buffer.m.offset = cam->dummy_frame.paddress;
 
-	ipu_clear_irq(IPU_IRQ_CSI0_OUT_EOF);
-	err = ipu_request_irq(IPU_IRQ_CSI0_OUT_EOF,
-			      csi_enc_callback, 0, "Mxc Camera", cam);
+	if (cam->csi == 0)
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI0_OUT_EOF;
+	else
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI1_OUT_EOF;
+	ipu_clear_irq(ipu_irq_csi_out_eof);
+	err = ipu_request_irq(ipu_irq_csi_out_eof,
+				csi_enc_callback, 0, "Mxc Camera", cam);
+
 	if (err != 0) {
-		printk(KERN_ERR "Error registering rot irq\n");
+		printk(KERN_ERR "Error registering eot irq for csi %d\n",cam->csi);
 		return err;
 	}
 
@@ -209,12 +232,24 @@ static int csi_enc_disabling_tasks(void *private)
 {
 	cam_data *cam = (cam_data *) private;
 	int err = 0;
+	ipu_channel_t channel;
+	int ipu_irq_csi_out_eof;
 
-	ipu_free_irq(IPU_IRQ_CSI0_OUT_EOF, cam);
+	if (cam->csi == 0)
+	{
+		channel = CSI_MEM0;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI0_OUT_EOF;
+	}
+	else
+	{
+		channel = CSI_MEM1;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI1_OUT_EOF;
+	}
 
-	err = ipu_disable_channel(CSI_MEM, true);
+	ipu_free_irq(ipu_irq_csi_out_eof, cam);
+	err = ipu_disable_channel(channel, true);
 
-	ipu_uninit_channel(CSI_MEM);
+	ipu_uninit_channel(channel);
 
 	if (cam->dummy_frame.vaddress != 0) {
 		dma_free_coherent(0, cam->dummy_frame.buffer.length,
diff --git a/drivers/media/video/mxc/capture/ipu_prp_enc.c b/drivers/media/video/mxc/capture/ipu_prp_enc.c
index 4b5426cb887d..0df8050ad7de 100644
--- a/drivers/media/video/mxc/capture/ipu_prp_enc.c
+++ b/drivers/media/video/mxc/capture/ipu_prp_enc.c
@@ -19,12 +19,14 @@
  * @ingroup IPU
  */
 
+#include <linux/types.h>
 #include <linux/dma-mapping.h>
 #include <linux/ipu.h>
 #include "mxc_v4l2_capture.h"
 #include "ipu_prp_sw.h"
 
 #ifdef CAMERA_DBG
+	extern void ipu_dump_registers(void);
 	#define CAMERA_TRACE(x) (printk)x
 #else
 	#define CAMERA_TRACE(x)
@@ -266,11 +268,10 @@ static int prp_enc_setup(cam_data * cam)
  *
  * @return  status
  */
-static int prp_enc_eba_update(dma_addr_t eba, int *buffer_num)
+static int prp_enc_eba_update(int csi, dma_addr_t eba, int *buffer_num)
 {
 	int err = 0;
 
-	pr_debug("eba %x\n", eba);
 	if (grotation >= IPU_ROTATE_90_RIGHT) {
 		err = ipu_update_channel_buffer(MEM_ROT_ENC_MEM,
 						IPU_OUTPUT_BUFFER, *buffer_num,
@@ -294,6 +295,11 @@ static int prp_enc_eba_update(dma_addr_t eba, int *buffer_num)
 	}
 
 	*buffer_num = (*buffer_num == 0) ? 1 : 0;
+
+#ifdef CAMERA_DBG
+	ipu_dump_registers ();
+#endif
+
 	return 0;
 }
 
@@ -350,7 +356,6 @@ static int prp_enc_disabling_tasks(void *private)
 	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
 		ipu_unlink_channels(CSI_PRP_ENC_MEM, MEM_ROT_ENC_MEM);
 	}
-
 	err = ipu_disable_channel(CSI_PRP_ENC_MEM, true);
 	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
 		err |= ipu_disable_channel(MEM_ROT_ENC_MEM, true);
diff --git a/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
index 7f0984c42950..9f8078d558b0 100644
--- a/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
+++ b/drivers/media/video/mxc/capture/ipu_prp_vf_sdc_bg.c
@@ -182,6 +182,7 @@ static int prpvf_start(void *private)
 		printk(KERN_ERR "Error initializing CSI_PRP_VF_MEM\n");
 		goto out_3;
 	}
+
 	err = ipu_init_channel(MEM_ROT_VF_MEM, NULL);
 	if (err != 0) {
 		printk(KERN_ERR "Error MEM_ROT_VF_MEM channel\n");
@@ -200,6 +201,7 @@ static int prpvf_start(void *private)
 	}
 
 	if (cam->vf_rotation >= IPU_ROTATE_90_RIGHT) {
+
 		err = ipu_init_channel_buffer(MEM_ROT_VF_MEM, IPU_OUTPUT_BUFFER,
 					      format,
 					      vf.csi_prp_vf_mem.out_height,
diff --git a/drivers/media/video/mxc/capture/ipu_still.c b/drivers/media/video/mxc/capture/ipu_still.c
index 348bf2b9b564..22cf3f51e1cb 100644
--- a/drivers/media/video/mxc/capture/ipu_still.c
+++ b/drivers/media/video/mxc/capture/ipu_still.c
@@ -26,6 +26,9 @@
 #include "ipu_prp_sw.h"
 
 static int callback_eof_flag;
+#ifndef CONFIG_MXC_IPU_V1
+static int buffer_num;
+#endif
 
 #ifdef CONFIG_MXC_IPU_V1
 static int callback_flag;
@@ -42,10 +45,10 @@ static int callback_flag;
  */
 static irqreturn_t prp_csi_eof_callback(int irq, void *dev_id)
 {
-	if (callback_flag == 2) {
-		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
+	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+			  callback_flag%2 ? 1 : 0);
+	if (callback_flag == 0)
 		ipu_enable_channel(CSI_MEM);
-	}
 
 	callback_flag++;
 	return IRQ_HANDLED;
@@ -65,9 +68,12 @@ static irqreturn_t prp_still_callback(int irq, void *dev_id)
 	cam_data *cam = (cam_data *) dev_id;
 
 	callback_eof_flag++;
-	if (callback_eof_flag < 5)
-		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
-	else {
+	if (callback_eof_flag < 5) {
+#ifndef CONFIG_MXC_IPU_V1
+		buffer_num = (buffer_num == 0) ? 1 : 0;
+		ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, buffer_num);
+#endif
+	} else {
 		cam->still_counter++;
 		wake_up_interruptible(&cam->still_queue);
 	}
@@ -87,6 +93,8 @@ static int prp_still_start(void *private)
 	u32 pixel_fmt;
 	int err;
 	ipu_channel_params_t params;
+	ipu_channel_t channel;
+	int ipu_irq_csi_out_eof;
 
 	if (cam->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420)
 		pixel_fmt = IPU_PIX_FMT_YUV420P;
@@ -113,20 +121,32 @@ static int prp_still_start(void *private)
 
 	ipu_csi_enable_mclk_if(CSI_MCLK_RAW, cam->csi, true, true);
 
+	if (cam->csi == 0) {
+		channel = CSI_MEM0;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI0_OUT_EOF;
+	}
+	else {
+		channel = CSI_MEM1;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI1_OUT_EOF;
+	}
+
 	memset(&params, 0, sizeof(params));
-	err = ipu_init_channel(CSI_MEM, &params);
+	params.csi_mem.csi = cam->csi;
+	err = ipu_init_channel(channel, &params);
 	if (err != 0)
 		return err;
 
-	err = ipu_init_channel_buffer(CSI_MEM, IPU_OUTPUT_BUFFER,
+	err = ipu_init_channel_buffer(channel, IPU_OUTPUT_BUFFER,
 				      pixel_fmt, cam->v2f.fmt.pix.width,
 				      cam->v2f.fmt.pix.height,
 				      cam->v2f.fmt.pix.width, IPU_ROTATE_NONE,
-				      cam->still_buf, 0, 0, 0);
+				      cam->still_buf[0], cam->still_buf[1],
+				      0, 0);
 	if (err != 0)
 		return err;
 
 #ifdef CONFIG_MXC_IPU_V1
+	ipu_clear_irq(IPU_IRQ_SENSOR_OUT_EOF);
 	err = ipu_request_irq(IPU_IRQ_SENSOR_OUT_EOF, prp_still_callback,
 			      0, "Mxc Camera", cam);
 	if (err != 0) {
@@ -135,6 +155,7 @@ static int prp_still_start(void *private)
 	}
 	callback_flag = 0;
 	callback_eof_flag = 0;
+	ipu_clear_irq(IPU_IRQ_SENSOR_EOF);
 	err = ipu_request_irq(IPU_IRQ_SENSOR_EOF, prp_csi_eof_callback,
 			      0, "Mxc Camera", NULL);
 	if (err != 0) {
@@ -142,8 +163,9 @@ static int prp_still_start(void *private)
 		return err;
 	}
 #else
-	ipu_clear_irq(IPU_IRQ_CSI0_OUT_EOF);
-	err = ipu_request_irq(IPU_IRQ_CSI0_OUT_EOF, prp_still_callback,
+
+	ipu_clear_irq(ipu_irq_csi_out_eof);
+	err = ipu_request_irq(ipu_irq_csi_out_eof, prp_still_callback,
 			      0, "Mxc Camera", cam);
 	if (err != 0) {
 		printk(KERN_ERR "Error registering irq.\n");
@@ -151,9 +173,9 @@ static int prp_still_start(void *private)
 	}
 
 	callback_eof_flag = 0;
+	ipu_select_buffer(channel, IPU_OUTPUT_BUFFER, 0);
+	ipu_enable_channel(channel);
 
-	ipu_select_buffer(CSI_MEM, IPU_OUTPUT_BUFFER, 0);
-	ipu_enable_channel(CSI_MEM);
 	ipu_enable_csi(cam->csi);
 #endif
 
@@ -170,17 +192,28 @@ static int prp_still_stop(void *private)
 {
 	cam_data *cam = (cam_data *) private;
 	int err = 0;
+	ipu_channel_t channel;
+	int ipu_irq_csi_out_eof;
+
+	if (cam->csi == 0) {
+		channel = CSI_MEM0;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI0_OUT_EOF;
+	}
+	else {
+		channel = CSI_MEM1;
+		ipu_irq_csi_out_eof = IPU_IRQ_CSI1_OUT_EOF;
+	}
 
 #ifdef CONFIG_MXC_IPU_V1
 	ipu_free_irq(IPU_IRQ_SENSOR_EOF, NULL);
 	ipu_free_irq(IPU_IRQ_SENSOR_OUT_EOF, cam);
 #else
-	ipu_free_irq(IPU_IRQ_CSI0_OUT_EOF, cam);
+	ipu_free_irq(ipu_irq_csi_out_eof, cam);
 #endif
 
 	ipu_disable_csi(cam->csi);
-	ipu_disable_channel(CSI_MEM, true);
-	ipu_uninit_channel(CSI_MEM);
+	ipu_disable_channel(channel, true);
+	ipu_uninit_channel(channel);
 	ipu_csi_enable_mclk_if(CSI_MCLK_RAW, cam->csi, false, false);
 
 	return err;
diff --git a/drivers/media/video/mxc/capture/mt9v111.c b/drivers/media/video/mxc/capture/mt9v111.c
index c95a20683924..dfdd455db3dd 100644
--- a/drivers/media/video/mxc/capture/mt9v111.c
+++ b/drivers/media/video/mxc/capture/mt9v111.c
@@ -18,6 +18,9 @@
  *
  * @ingroup Camera
  */
+
+//#define MT9V111_DEBUG
+
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -40,7 +43,6 @@ static mt9v111_conf mt9v111_device;
 /*!
  * Holds the current frame rate.
  */
-static int reset_frame_rate = MT9V111_FRAME_RATE;
 
 struct sensor {
 	const struct mt9v111_platform_data *platform_data;
@@ -49,33 +51,45 @@ struct sensor {
 	struct v4l2_pix_format pix;
 	struct v4l2_captureparm streamcap;
 	bool on;
+	bool used;
 
 	/* control settings */
 	int brightness;
-	int hue;
-	int contrast;
 	int saturation;
-	int red;
-	int green;
-	int blue;
+	int sharpness;
+	int gain;
 	int ae_mode;
 
-} mt9v111_data;
-
-extern void gpio_sensor_active(void);
-extern void gpio_sensor_inactive(void);
+};
 
 static int mt9v111_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id);
 static int mt9v111_remove(struct i2c_client *client);
 
 static const struct i2c_device_id mt9v111_id[] = {
-	{"mt9v111", 0},
+	{"mt9v111_1", 2},
+	{"mt9v111_2", 3},
 	{},
 };
 
+struct sensor mt9v111_data[ARRAY_SIZE(mt9v111_id)-1];
+
 MODULE_DEVICE_TABLE(i2c, mt9v111_id);
 
+static int mt9v111_suspend(struct i2c_client *client, pm_message_t mesg)
+{
+	pr_debug("In mt9v111_suspend\n");
+
+	return 0;
+}
+
+static int mt9v111_resume(struct i2c_client *client)
+{
+	pr_debug("In mt9v111_resume\n");
+
+	return 0;
+}
+
 static struct i2c_driver mt9v111_i2c_driver = {
 	.driver = {
 		   .owner = THIS_MODULE,
@@ -84,37 +98,52 @@ static struct i2c_driver mt9v111_i2c_driver = {
 	.probe = mt9v111_probe,
 	.remove = mt9v111_remove,
 	.id_table = mt9v111_id,
-/* To add power management add .suspend and .resume functions */
+	.suspend = mt9v111_suspend,
+	.resume = mt9v111_resume,
 };
 
 /*
  * Function definitions
  */
 
-#ifdef MT9V111_DEBUG
-static inline int mt9v111_read_reg(u8 reg)
+static int mt9v111_id_from_name ( const char * name )
+{
+	int id = -1;
+
+	if( name == NULL || ( strlen(name) < strlen("mt9v111_n") ) )
+		return -1;
+
+	id = (int)simple_strtol(name+strlen("mt9v111_"),NULL,0) - 1;
+	if( id >= ARRAY_SIZE(mt9v111_id) ) {
+		printk("Invalid sensor index %d for %s\n",id,name);
+		return -1;
+	}
+
+	return id;
+}
+
+static inline int mt9v111_read_reg(int sensorid , u8 reg)
 {
-	int val = i2c_smbus_read_word_data(mt9v111_data.i2c_client, reg);
+	int val = i2c_smbus_read_word_data(mt9v111_data[sensorid].i2c_client, reg);
 	if (val != -1)
 		val = cpu_to_be16(val);
 	return val;
 }
-#endif
 
 /*!
  * Writes to the register via I2C.
  */
-static inline int mt9v111_write_reg(u8 reg, u16 val)
+static inline int mt9v111_write_reg(int sensorid , u8 reg, u16 val)
 {
-	pr_debug("In mt9v111_write_reg (0x%x, 0x%x)\n", reg, val);
+	pr_debug("[%d] In mt9v111_write_reg (0x%x, 0x%x)\n", sensorid , reg, val);
 	pr_debug("   write reg %x val %x.\n", reg, val);
 
-	return i2c_smbus_write_word_data(mt9v111_data.i2c_client,
+	return i2c_smbus_write_word_data(mt9v111_data[sensorid].i2c_client,
 					 reg, cpu_to_be16(val));
 }
 
 /*!
- * Initialize mt9v111_sensor_lib
+ * Initialize mt9v111_sensor_lib_datasheet
  * Libarary for Sensor configuration through I2C
  *
  * @param       coreReg       Core Registers
@@ -122,7 +151,7 @@ static inline int mt9v111_write_reg(u8 reg, u16 val)
  *
  * @return status
  */
-static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
+static u8 mt9v111_sensor_lib_datasheet(int sensorid , mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
 {
 	u8 reg;
 	u16 data;
@@ -130,200 +159,103 @@ static u8 mt9v111_sensor_lib(mt9v111_coreReg * coreReg, mt9v111_IFPReg * ifpReg)
 
 	pr_debug("In mt9v111_sensor_lib\n");
 
+	/* IFP R51(0x33)=5137,R57(0x39)=290,R59(0x3B)=1068,R62(0x3E)=4095,R89(0x59)=504,R90(0x5A)=605,R92(0x5C)=8222,R93(0x5D)=10021,R100(0x64)=4477 */
+
 	/*
 	 * setup to IFP registers
 	 */
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = ifpReg->addrSpaceSel;
-	mt9v111_write_reg(reg, data);
-
-	/* Operation Mode Control */
-	reg = MT9V111I_MODE_CONTROL;
-	data = ifpReg->modeControl;
-	mt9v111_write_reg(reg, data);
-
-	/* Output format */
-	reg = MT9V111I_FORMAT_CONTROL;
-	data = ifpReg->formatControl;	/* Set bit 12 */
-	mt9v111_write_reg(reg, data);
-
-	/* AE limit 4 */
-	reg = MT9V111I_SHUTTER_WIDTH_LIMIT_AE;
-	data = ifpReg->gainLimitAE;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111I_OUTPUT_FORMAT_CTRL2;
-	data = ifpReg->outputFormatCtrl2;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111I_AE_SPEED;
-	data = ifpReg->AESpeed;
-	mt9v111_write_reg(reg, data);
-
-	/* output image size */
-	reg = MT9V111i_H_PAN;
-	data = 0x8000 | ifpReg->HPan;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_H_ZOOM;
-	data = 0x8000 | ifpReg->HZoom;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_H_SIZE;
-	data = 0x8000 | ifpReg->HSize;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_V_PAN;
-	data = 0x8000 | ifpReg->VPan;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
-	reg = MT9V111i_V_ZOOM;
-	data = 0x8000 | ifpReg->VZoom;
-	mt9v111_write_reg(reg, data);
+	reg = MT9V111I_LIMIT_SHARP_SATU_CTRL;
+	data = ifpReg->limitSharpSatuCtrl;
+	mt9v111_write_reg(sensorid,reg, data);
 
-	reg = MT9V111i_V_SIZE;
-	data = 0x8000 | ifpReg->VSize;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111i_H_PAN;
-	data = ~0x8000 & ifpReg->HPan;
-	mt9v111_write_reg(reg, data);
-#if 0
 	reg = MT9V111I_UPPER_SHUTTER_DELAY_LIM;
 	data = ifpReg->upperShutterDelayLi;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_IPF_BLACK_LEVEL_SUB;
+	data = ifpReg->ipfBlackLevelSub;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_GAIN_THRE_CCAM_ADJ;
+	data = ifpReg->agimnThreCamAdj;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	reg = MT9V111I_SHUTTER_60;
 	data = ifpReg->shutter_width_60;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_AUTO_EXPOSURE_17;
+	data = ifpReg->auto_exposure_17;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	reg = MT9V111I_SEARCH_FLICK_60;
 	data = ifpReg->search_flicker_60;
-	mt9v111_write_reg(reg, data);
-#endif
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_RESERVED93;
+	data = ifpReg->reserved93;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111I_RESERVED100;
+	data = ifpReg->reserved100;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	/*
 	 * setup to sensor core registers
 	 */
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = coreReg->addressSelect;
-	mt9v111_write_reg(reg, data);
-
-	/* enable changes and put the Sync bit on */
-	reg = MT9V111S_OUTPUT_CTRL;
-	data = MT9V111S_OUTCTRL_SYNC | MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
-	/* min PIXCLK - Default */
-	reg = MT9V111S_PIXEL_CLOCK_SPEED;
-	data = coreReg->pixelClockSpeed;
-	mt9v111_write_reg(reg, data);
+	/* Core R5=46, R7[4]=0 (DEFAULT) ,R33=58369*/
 
-	/* Setup image flipping / Dark rows / row/column skip */
-	reg = MT9V111S_READ_MODE;
-	data = coreReg->readMode;
-	mt9v111_write_reg(reg, data);
-
-	/* zoom 0 */
-	reg = MT9V111S_DIGITAL_ZOOM;
-	data = coreReg->digitalZoom;
-	mt9v111_write_reg(reg, data);
-
-	/* min H-blank */
 	reg = MT9V111S_HOR_BLANKING;
 	data = coreReg->horizontalBlanking;
-	mt9v111_write_reg(reg, data);
-
-	/* min V-blank */
-	reg = MT9V111S_VER_BLANKING;
-	data = coreReg->verticalBlanking;
-	mt9v111_write_reg(reg, data);
-
-	reg = MT9V111S_SHUTTER_WIDTH;
-	data = coreReg->shutterWidth;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
-	reg = MT9V111S_SHUTTER_DELAY;
-	data = ifpReg->upperShutterDelayLi;
-	mt9v111_write_reg(reg, data);
-
-	/* changes become effective */
-	reg = MT9V111S_OUTPUT_CTRL;
-	data = MT9V111S_OUTCTRL_CHIP_ENABLE | 0x3000;
-	mt9v111_write_reg(reg, data);
+	reg = MT9V111S_RESERVED33;
+	data = coreReg->reserved33;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	return error;
 }
 
-/*!
- * MT9V111 frame rate calculate
- *
- * @param frame_rate       int *
- * @param mclk             int
- * @return  None
- */
-static void mt9v111_rate_cal(int *frame_rate, int mclk)
+void mt9v111_config_datasheet(void)
 {
-	int num_clock_per_row;
-	int max_rate = 0;
+	pr_debug("In mt9v111_config_datasheet\n");
 
-	pr_debug("In mt9v111_rate_cal\n");
+	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
 
-	num_clock_per_row = (MT9V111_MAX_WIDTH + 114 + MT9V111_HORZBLANK_MIN)
-			* 2;
-	max_rate = mclk / (num_clock_per_row *
-			   (MT9V111_MAX_HEIGHT + MT9V111_VERTBLANK_DEFAULT));
+	/* MT9V111I_ADDR_SPACE_SEL */
+	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
 
-	if ((*frame_rate > max_rate) || (*frame_rate == 0)) {
-		*frame_rate = max_rate;
-	}
+	/* Recommended values for 30fps @ 27MHz from datasheet*/
 
-	mt9v111_device.coreReg->verticalBlanking
-	    = mclk / (*frame_rate * num_clock_per_row) - MT9V111_MAX_HEIGHT;
+	/* Core R5=132, R6=10 , R7[4]=0 (DEFAULT) ,R33=58369*/
 
-	reset_frame_rate = *frame_rate;
-}
+	mt9v111_device.coreReg->horizontalBlanking = 132;
+	mt9v111_device.coreReg->verticalBlanking = 10;
+	mt9v111_device.coreReg->reserved33 = 58369;
 
-/*!
- * MT9V111 sensor configuration
- */
-void mt9v111_config(void)
-{
-	pr_debug("In mt9v111_config\n");
+	/* IFP R51(0x33)=5137,R57(0x39)=290,R59(0x3B)=1068,R62(0x3E)=4095,R89(0x59)=504,R90(0x5A)=605,R92(0x5C)=8222,R93(0x5D)=10021,R100(0x64)=4477 */
 
-	mt9v111_device.coreReg->addressSelect = MT9V111I_SEL_SCA;
-	mt9v111_device.ifpReg->addrSpaceSel = MT9V111I_SEL_IFP;
+	mt9v111_device.ifpReg->limitSharpSatuCtrl = 5137;
+	mt9v111_device.ifpReg->upperShutterDelayLi = 290;
+	mt9v111_device.ifpReg->ipfBlackLevelSub = 1068;
+	mt9v111_device.ifpReg->agimnThreCamAdj = 4095;
 
-	mt9v111_device.coreReg->windowHeight = MT9V111_WINHEIGHT;
-	mt9v111_device.coreReg->windowWidth = MT9V111_WINWIDTH;
-	mt9v111_device.coreReg->zoomColStart = 0;
-	mt9v111_device.coreReg->zomRowStart = 0;
-	mt9v111_device.coreReg->digitalZoom = 0x0;
-
-	mt9v111_device.coreReg->verticalBlanking = MT9V111_VERTBLANK_DEFAULT;
-	mt9v111_device.coreReg->horizontalBlanking = MT9V111_HORZBLANK_MIN;
-	mt9v111_device.coreReg->pixelClockSpeed = 0;
-	mt9v111_device.coreReg->readMode = 0xd0a1;
-
-	mt9v111_device.ifpReg->outputFormatCtrl2 = 0;
-	mt9v111_device.ifpReg->gainLimitAE = 0x300;
-	mt9v111_device.ifpReg->AESpeed = 0x80;
-
-	/* here is the default value */
-	mt9v111_device.ifpReg->formatControl = 0xc800;
-	mt9v111_device.ifpReg->modeControl = 0x708e;
-	mt9v111_device.ifpReg->awbSpeed = 0x4514;
-	mt9v111_device.coreReg->shutterWidth = 0xf8;
-
-	/* output size */
-	mt9v111_device.ifpReg->HPan = 0;
-	mt9v111_device.ifpReg->HZoom = MT9V111_MAX_WIDTH;
-	mt9v111_device.ifpReg->HSize = MT9V111_MAX_WIDTH;
-	mt9v111_device.ifpReg->VPan = 0;
-	mt9v111_device.ifpReg->VZoom = MT9V111_MAX_HEIGHT;
-	mt9v111_device.ifpReg->VSize = MT9V111_MAX_HEIGHT;
+	mt9v111_device.ifpReg->shutter_width_60 = 504;
+	mt9v111_device.ifpReg->auto_exposure_17 = 605;
+	mt9v111_device.ifpReg->search_flicker_60 = 8222;
+	mt9v111_device.ifpReg->reserved93 = 10021;
+	mt9v111_device.ifpReg->reserved100 = 4477;
 }
 
+
 /*!
  * mt9v111 sensor set saturtionn
  *
@@ -331,7 +263,7 @@ void mt9v111_config(void)
 
  * @return  Error code of 0.
  */
-static int mt9v111_set_saturation(int saturation)
+static int mt9v111_set_saturation(int sensorid , int saturation)
 {
 	u8 reg;
 	u16 data;
@@ -357,6 +289,9 @@ static int mt9v111_set_saturation(int saturation)
 	case 25:
 		mt9v111_device.ifpReg->awbSpeed = 0x6514;
 		break;
+	case 0:
+		mt9v111_device.ifpReg->awbSpeed = 0x7514;
+		break;
 	default:
 		mt9v111_device.ifpReg->awbSpeed = 0x4514;
 		break;
@@ -364,12 +299,348 @@ static int mt9v111_set_saturation(int saturation)
 
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = mt9v111_device.ifpReg->addrSpaceSel;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
 	/* Operation Mode Control */
 	reg = MT9V111I_AWB_SPEED;
 	data = mt9v111_device.ifpReg->awbSpeed;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+#if 0
+/*!
+ * mt9v111 sensor set digital zoom
+ *
+ * @param on/off   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalzoom(int sensorid , unsigned int on)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_digitalzoom(%d)\n",on);
+
+	if( on > 1 )
+	    return -1;
+
+	mt9v111_device.coreReg->digitalZoom = on;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.coreReg->addressSelect;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111S_DIGITAL_ZOOM;
+	data = mt9v111_device.coreReg->digitalZoom;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital pan
+ *
+ * @param pan_level   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalpan (int sensorid , int pan_level)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_digitalpan(%d)\n",
+		pan_level);
+
+	mt9v111_device.ifpReg->HPan = 8;
+	if (pan_level & 0xFFFF0000) {
+		pan_level = (0xFFFFFFFF - pan_level);
+		pan_level = pan_level / 0x14;
+		mt9v111_device.ifpReg->HPan =
+			mt9v111_device.ifpReg->HPan - (pan_level & 0x3FF);
+	} else {
+		pan_level = pan_level / 0x14;
+		mt9v111_device.ifpReg->HPan =
+			mt9v111_device.ifpReg->HPan + (pan_level - 1);
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111i_H_PAN;
+	data = mt9v111_device.ifpReg->HPan;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital tilt
+ *
+ * @param tilt_level   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitaltilt (int sensorid , int tilt_level)
+{
+	u8 reg;
+	u16 data;
+	pr_debug("In mt9v111_set_digitaltilt(%d)\n",
+		tilt_level);
+
+	mt9v111_device.ifpReg->VPan = 8;
+        if( tilt_level & 0xFFFF0000 ) {
+				tilt_level = (0xFFFFFFFF - tilt_level);
+				tilt_level = tilt_level / 0x14;
+                mt9v111_device.ifpReg->VPan = mt9v111_device.ifpReg->VPan - (tilt_level & 0x3FF);
+        }
+        else {
+				tilt_level = tilt_level / 0x14;
+                mt9v111_device.ifpReg->VPan = mt9v111_device.ifpReg->VPan + (tilt_level - 1);
+        }
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111i_V_PAN;
+	data = mt9v111_device.ifpReg->VPan;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set output resolution
+ *
+ * @param resolution   res
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_outputresolution(int sensorid , MT9V111_OutputResolution res)
+{
+	u8 reg;
+	u16 data;
+	int zoom = 0;
+
+	pr_debug("In mt9v111_set_outputresolution(%d)\n",res);
+
+	switch (res) {
+	    case MT9V111_OutputResolution_VGA:
+			/* 640x480 */
+			mt9v111_device.ifpReg->HSize = 0x0280;
+			mt9v111_device.ifpReg->VSize = 0x01E0;
+			break;
+
+	    case MT9V111_OutputResolution_QVGA:
+			/* 320x240 */
+			mt9v111_device.ifpReg->HSize = 0x0140;
+			mt9v111_device.ifpReg->VSize = 0x00F0;
+			break;
+
+	    case MT9V111_OutputResolution_CIF:
+			/* 352x288 */
+			mt9v111_device.ifpReg->HSize = 0x0160;
+			mt9v111_device.ifpReg->VSize = 0x0120;
+			mt9v111_device.ifpReg->HZoom = 0x0160;
+			mt9v111_device.ifpReg->VZoom = 0x0120;
+			zoom = 1;
+			break;
+
+	    case MT9V111_OutputResolution_QCIF:
+			/* 176X220 */
+			mt9v111_device.ifpReg->HSize = 0x00B0;
+			mt9v111_device.ifpReg->VSize = 0x0090;
+			mt9v111_device.ifpReg->HZoom = 0x00B0;
+			mt9v111_device.ifpReg->VZoom = 0x0090;
+			zoom = 1;
+			break;
+
+	    case MT9V111_OutputResolution_QQVGA:
+			/* 2048*1536 */
+			mt9v111_device.ifpReg->HSize = 0x00A0;
+			mt9v111_device.ifpReg->VSize = 0x0078;
+			mt9v111_device.ifpReg->HZoom = 0x00A0;
+			mt9v111_device.ifpReg->VZoom = 0x0078;
+			zoom = 1;
+			break;
+
+	    case MT9V111_OutputResolution_SXGA:
+		/* 1280x1024 */
+			break;
+
+		default:
+			break;
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111i_V_SIZE;
+	data = mt9v111_device.ifpReg->VSize;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	reg = MT9V111i_H_SIZE;
+	data = mt9v111_device.ifpReg->HSize;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	if ( zoom ) {
+	    reg = MT9V111i_V_ZOOM;
+	    data = mt9v111_device.ifpReg->VZoom;
+	    mt9v111_write_reg(sensorid,reg, data);
+
+	    reg = MT9V111i_H_ZOOM;
+	    data = mt9v111_device.ifpReg->HZoom;
+	    mt9v111_write_reg(sensorid,reg, data);
+	}
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital flash
+ *
+ * @param flash_level   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalflash (int sensorid , int flash_level)
+{
+	u8 reg;
+	u16 data = mt9v111_read_reg(sensorid,MT9V111i_FLASH_CTRL);
+	pr_debug("In mt9v111_set_digitalflash(%d)\n",
+			flash_level);
+
+	if(flash_level) {
+	    data &= (0xFF00);
+	    data |= ((flash_level & 0x00FF) | (1<<13));
+	}
+	else {
+	    data &= ~(1<<13);
+	}
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111i_FLASH_CTRL;
+	mt9v111_device.ifpReg->flashCtrl = data;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital monochrome
+ *
+ * @param on   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalmonochrome (int sensorid , int on)
+{
+	u8 reg;
+	u16 data = mt9v111_read_reg(sensorid,MT9V111I_FORMAT_CONTROL);
+	pr_debug("In mt9v111_set_digitalmonochrome(%d)\n",
+			on);
+
+        /* clear the monochrome bit field */
+        data &= ~(1<<5);
+
+         /* enable or disable monochrome mode */
+         if( on )
+                 data |= (0<<5);
+         else
+                 data |= (1<<5);
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	/* Operation Mode Control */
+	reg = MT9V111I_FORMAT_CONTROL;
+	mt9v111_device.ifpReg->formatControl = data;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+#endif
+
+/*!
+ * mt9v111 sensor set digital sharpness
+ *
+ * @param value   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalsharpness (int sensorid , int value)
+{
+	u8 reg;
+	u16 data ;
+
+	pr_debug("In mt9v111_set_digitalsharpness(%d)\n",value);
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	data = mt9v111_read_reg(sensorid,MT9V111I_APERTURE_GAIN);
+
+	/* erase current and remove auto reduce sharpness in low light */
+	 data &= ~(0x000F);
+	 data |= (value & (0x000F));
+	 if( data > (0x000F) )
+			 return -1;
+
+	/* Operation Mode Control */
+	reg = MT9V111I_APERTURE_GAIN;
+	mt9v111_device.ifpReg->apertureGain = data;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	return 0;
+}
+
+/*!
+ * mt9v111 sensor set digital brightness
+ *
+ * @param value   int
+
+ * @return  0 on success, -1 on error.
+ */
+static int mt9v111_set_digitalbrightness (int sensorid , int value)
+{
+	u8 reg;
+	u16 data;
+	u32 max_brightness, min_brightness;
+
+	data = mt9v111_read_reg(sensorid,MT9V111I_CLIP_LIMIT_OUTPUT_LUMI);
+	max_brightness = data >> 8;
+	min_brightness = (u8)data;
+
+	if( value > max_brightness )
+		value = max_brightness;
+	else if( value < min_brightness )
+		value = min_brightness;
+
+	reg = MT9V111I_ADDR_SPACE_SEL;
+	data = mt9v111_device.ifpReg->addrSpaceSel;
+	mt9v111_write_reg(sensorid,reg, data);
+
+	data = mt9v111_read_reg(sensorid,MT9V111I_AE_PRECISION_TARGET);
+	data &= 0xFF00; /* Clear target luminance */
+	data |= ((u8)value );
+
+	/* Operation Mode Control */
+	reg = MT9V111I_AE_PRECISION_TARGET;
+	mt9v111_device.ifpReg->AEPrecisionTarget = data;
+	mt9v111_write_reg(sensorid,reg, data);
 
 	return 0;
 }
@@ -380,7 +651,7 @@ static int mt9v111_set_saturation(int saturation)
  * @param ae_mode      int
  * @return  Error code of 0 (no Error)
  */
-static int mt9v111_set_ae_mode(int ae_mode)
+static int mt9v111_set_ae_mode(int sensorid , int ae_mode)
 {
 	u8 reg;
 	u16 data;
@@ -408,19 +679,20 @@ static int mt9v111_set_ae_mode(int ae_mode)
 	/* V4L2_EXPOSURE_MANUAL = 1 needs register setting of 0x308E */
 	mt9v111_device.ifpReg->modeControl &= 0x3fff;
 	mt9v111_device.ifpReg->modeControl |= (ae_mode & 0x03) << 14;
-	mt9v111_data.ae_mode = ae_mode;
+	mt9v111_data[sensorid].ae_mode = ae_mode;
 
 	reg = MT9V111I_ADDR_SPACE_SEL;
 	data = mt9v111_device.ifpReg->addrSpaceSel;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
 	reg = MT9V111I_MODE_CONTROL;
 	data = mt9v111_device.ifpReg->modeControl;
-	mt9v111_write_reg(reg, data);
+	mt9v111_write_reg(sensorid,reg, data);
 
 	return 0;
 }
 
+#if 0
 /*!
  * mt9v111 sensor get AE measurement window mode configuration
  *
@@ -435,6 +707,7 @@ static void mt9v111_get_ae_mode(int *ae_mode)
 		*ae_mode = (mt9v111_device.ifpReg->modeControl & 0xc) >> 2;
 	}
 }
+#endif
 
 #ifdef MT9V111_DEBUG
 /*!
@@ -442,33 +715,33 @@ static void mt9v111_get_ae_mode(int *ae_mode)
  *
  * @return none
  */
-static void mt9v111_test_pattern(bool flag)
+static void mt9v111_test_pattern(int sensorid , bool flag)
 {
 	u16 data;
 
 	/* switch to sensor registers */
-	mt9v111_write_reg(MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
+	mt9v111_write_reg(sensorid,MT9V111I_ADDR_SPACE_SEL, MT9V111I_SEL_SCA);
 
 	if (flag == true) {
 		testpattern = MT9V111S_OUTCTRL_TEST_MODE;
 
-		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) & 0xBF;
-		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
+		data = mt9v111_read_reg(sensorid,MT9V111S_ROW_NOISE_CTRL) & 0xBF;
+		mt9v111_write_reg(sensorid,MT9V111S_ROW_NOISE_CTRL, data);
 
-		mt9v111_write_reg(MT9V111S_TEST_DATA, 0);
+		mt9v111_write_reg(sensorid,MT9V111S_TEST_DATA, 0);
 
 		/* changes take effect */
 		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
-		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+		mt9v111_write_reg(sensorid,MT9V111S_OUTPUT_CTRL, data);
 	} else {
 		testpattern = 0;
 
-		data = mt9v111_read_reg(MT9V111S_ROW_NOISE_CTRL) | 0x40;
+		data = mt9v111_read_reg(sensorid,MT9V111S_ROW_NOISE_CTRL) | 0x40;
 		mt9v111_write_reg(MT9V111S_ROW_NOISE_CTRL, data);
 
 		/* changes take effect */
 		data = MT9V111S_OUTCTRL_CHIP_ENABLE | testpattern | 0x3000;
-		mt9v111_write_reg(MT9V111S_OUTPUT_CTRL, data);
+		mt9v111_write_reg(sensorid,MT9V111S_OUTPUT_CTRL, data);
 	}
 }
 #endif
@@ -507,6 +780,7 @@ static int ioctl_g_ifparm(struct v4l2_int_device *s, struct v4l2_ifparm *p)
 	p->u.bt656.clock_curr = MT9V111_MCLK;
 	p->if_type = V4L2_IF_TYPE_BT656;
 	p->u.bt656.mode = V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT;
+	p->u.bt656.bt_sync_correct = 1;	// translates to CSI ext vsync
 	p->u.bt656.clock_min = MT9V111_CLK_MIN;
 	p->u.bt656.clock_max = MT9V111_CLK_MAX;
 
@@ -534,10 +808,12 @@ static int ioctl_s_power(struct v4l2_int_device *s, int on)
 
 	sensor->on = on;
 
-	if (on)
-		gpio_sensor_active();
-	else
-		gpio_sensor_inactive();
+	if(on) {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */, true, true);
+	}
+	else {
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */, false, false);
+	}
 
 	return 0;
 }
@@ -554,19 +830,23 @@ static int ioctl_g_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 	int ret = 0;
 	struct v4l2_captureparm *cparm = &a->parm.capture;
 	/* s->priv points to mt9v111_data */
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
 
 	pr_debug("In mt9v111:ioctl_g_parm\n");
 
+	if( sensorid < 0 )
+		return ret;
+
 	switch (a->type) {
 	/* This is the only case currently handled. */
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 		pr_debug("   type is V4L2_BUF_TYPE_VIDEO_CAPTURE\n");
 		memset(a, 0, sizeof(*a));
 		a->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		cparm->capability = mt9v111_data.streamcap.capability;
+		cparm->capability = mt9v111_data[sensorid].streamcap.capability;
 		cparm->timeperframe =
-				mt9v111_data.streamcap.timeperframe;
-		cparm->capturemode = mt9v111_data.streamcap.capturemode;
+				mt9v111_data[sensorid].streamcap.timeperframe;
+		cparm->capturemode = mt9v111_data[sensorid].streamcap.capturemode;
 		ret = 0;
 		break;
 
@@ -605,9 +885,13 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 	int ret = 0;
 	struct v4l2_captureparm *cparm = &a->parm.capture;
 	/* s->priv points to mt9v111_data */
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
 
 	pr_debug("In mt9v111:ioctl_s_parm\n");
 
+	if( sensorid < 0 )
+		return ret;
+
 	switch (a->type) {
 	/* This is the only case currently handled. */
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
@@ -617,13 +901,13 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 		 * Changing the frame rate is not allowed on this
 		 *camera. */
 		if (cparm->timeperframe.denominator !=
-		    mt9v111_data.streamcap.timeperframe.denominator) {
+		    mt9v111_data[sensorid].streamcap.timeperframe.denominator) {
 			pr_err("ERROR: mt9v111: ioctl_s_parm: " \
 			       "This camera does not allow frame rate "
 			       "changes.\n");
 			ret = -EINVAL;
 		} else {
-			mt9v111_data.streamcap.timeperframe =
+			mt9v111_data[sensorid].streamcap.timeperframe =
 						cparm->timeperframe;
 		      /* Call any camera functions to match settings. */
 		}
@@ -635,7 +919,7 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 				"unsupported capture mode\n");
 			ret  = -EINVAL;
 		} else {
-			mt9v111_data.streamcap.capturemode =
+			mt9v111_data[sensorid].streamcap.capturemode =
 						cparm->capturemode;
 		      /* Call any camera functions to match settings. */
 		      /* Right now this camera only supports 1 mode. */
@@ -685,6 +969,7 @@ static int ioctl_g_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
 	return 0;
 }
 
+#if 0
 /*!
  * ioctl_queryctrl - V4L2 sensor interface handler for VIDIOC_QUERYCTRL ioctl
  * @s: pointer to standard V4L2 device structure
@@ -700,6 +985,7 @@ static int ioctl_queryctrl(struct v4l2_int_device *s, struct v4l2_queryctrl *qc)
 
 	return 0;
 }
+#endif
 
 /*!
  * ioctl_g_ctrl - V4L2 sensor interface handler for VIDIOC_G_CTRL ioctl
@@ -712,66 +998,29 @@ static int ioctl_queryctrl(struct v4l2_int_device *s, struct v4l2_queryctrl *qc)
  */
 static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 {
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+
 	pr_debug("In mt9v111:ioctl_g_ctrl\n");
 
+	if( sensorid < 0 )
+		return 0;
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		pr_debug("   V4L2_CID_BRIGHTNESS\n");
-		vc->value = mt9v111_data.brightness;
-		break;
-	case V4L2_CID_CONTRAST:
-		pr_debug("   V4L2_CID_CONTRAST\n");
-		vc->value = mt9v111_data.contrast;
+		vc->value = mt9v111_data[sensorid].brightness;
 		break;
 	case V4L2_CID_SATURATION:
 		pr_debug("   V4L2_CID_SATURATION\n");
-		vc->value = mt9v111_data.saturation;
-		break;
-	case V4L2_CID_HUE:
-		pr_debug("   V4L2_CID_HUE\n");
-		vc->value = mt9v111_data.hue;
-		break;
-	case V4L2_CID_AUTO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_DO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_DO_WHITE_BALANCE\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_RED_BALANCE:
-		pr_debug("   V4L2_CID_RED_BALANCE\n");
-		vc->value = mt9v111_data.red;
-		break;
-	case V4L2_CID_BLUE_BALANCE:
-		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
-		vc->value = mt9v111_data.blue;
-		break;
-	case V4L2_CID_GAMMA:
-		pr_debug("   V4L2_CID_GAMMA\n");
-		vc->value = 0;
+		vc->value = mt9v111_data[sensorid].saturation;
 		break;
 	case V4L2_CID_EXPOSURE:
 		pr_debug("   V4L2_CID_EXPOSURE\n");
-		vc->value = mt9v111_data.ae_mode;
-		break;
-	case V4L2_CID_AUTOGAIN:
-		pr_debug("   V4L2_CID_AUTOGAIN\n");
-		vc->value = 0;
+		vc->value = mt9v111_data[sensorid].ae_mode;
 		break;
 	case V4L2_CID_GAIN:
 		pr_debug("   V4L2_CID_GAIN\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_HFLIP:
-		pr_debug("   V4L2_CID_HFLIP\n");
-		vc->value = 0;
-		break;
-	case V4L2_CID_VFLIP:
-		pr_debug("   V4L2_CID_VFLIP\n");
-		vc->value = 0;
+		vc->value = mt9v111_data[sensorid].gain;
 		break;
 	default:
 		pr_debug("   Default case\n");
@@ -794,56 +1043,34 @@ static int ioctl_g_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 {
 	int retval = 0;
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
 
 	pr_debug("In mt9v111:ioctl_s_ctrl %d\n",
 		vc->id);
 
+	if( sensorid < 0 )
+		return retval;
+
 	switch (vc->id) {
 	case V4L2_CID_BRIGHTNESS:
 		pr_debug("   V4L2_CID_BRIGHTNESS\n");
-		break;
-	case V4L2_CID_CONTRAST:
-		pr_debug("   V4L2_CID_CONTRAST\n");
+		mt9v111_set_digitalbrightness(sensorid,vc->value);
+		mt9v111_data[sensorid].brightness = vc->value;
 		break;
 	case V4L2_CID_SATURATION:
 		pr_debug("   V4L2_CID_SATURATION\n");
-		retval = mt9v111_set_saturation(vc->value);
-		break;
-	case V4L2_CID_HUE:
-		pr_debug("   V4L2_CID_HUE\n");
-		break;
-	case V4L2_CID_AUTO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_AUTO_WHITE_BALANCE\n");
-		break;
-	case V4L2_CID_DO_WHITE_BALANCE:
-		pr_debug(
-			"   V4L2_CID_DO_WHITE_BALANCE\n");
-		break;
-	case V4L2_CID_RED_BALANCE:
-		pr_debug("   V4L2_CID_RED_BALANCE\n");
-		break;
-	case V4L2_CID_BLUE_BALANCE:
-		pr_debug("   V4L2_CID_BLUE_BALANCE\n");
-		break;
-	case V4L2_CID_GAMMA:
-		pr_debug("   V4L2_CID_GAMMA\n");
+		retval = mt9v111_set_saturation(sensorid,vc->value);
+		mt9v111_data[sensorid].saturation = vc->value;
 		break;
 	case V4L2_CID_EXPOSURE:
 		pr_debug("   V4L2_CID_EXPOSURE\n");
-		retval = mt9v111_set_ae_mode(vc->value);
-		break;
-	case V4L2_CID_AUTOGAIN:
-		pr_debug("   V4L2_CID_AUTOGAIN\n");
+		retval = mt9v111_set_ae_mode(sensorid,vc->value);
+		mt9v111_data[sensorid].ae_mode = vc->value;
 		break;
 	case V4L2_CID_GAIN:
 		pr_debug("   V4L2_CID_GAIN\n");
-		break;
-	case V4L2_CID_HFLIP:
-		pr_debug("   V4L2_CID_HFLIP\n");
-		break;
-	case V4L2_CID_VFLIP:
-		pr_debug("   V4L2_CID_VFLIP\n");
+		mt9v111_set_digitalsharpness(sensorid,vc->value);
+		mt9v111_data[sensorid].gain = vc->value;
 		break;
 	default:
 		pr_debug("   Default case\n");
@@ -854,13 +1081,41 @@ static int ioctl_s_ctrl(struct v4l2_int_device *s, struct v4l2_control *vc)
 	return retval;
 }
 
+static void mt9v111_ifp_reset ( int sensorid )
+{
+	mt9v111_write_reg(sensorid,MT9V111S_ADDR_SPACE_SEL, 0x0001);
+	mt9v111_write_reg(sensorid,MT9V111I_SOFT_RESET, 0x0001);
+	msleep(100);
+	mt9v111_write_reg(sensorid,MT9V111I_SOFT_RESET, 0x0000);
+	msleep(100);
+}
+
+static void mt9v111_sensor_reset ( int sensorid )
+{
+	mt9v111_write_reg(sensorid,MT9V111S_ADDR_SPACE_SEL, 0x0004);
+	mt9v111_write_reg(sensorid,MT9V111S_RESET, 0x0001);
+	msleep(100);
+	mt9v111_write_reg(sensorid,MT9V111S_RESET, 0x0000);
+	msleep(100);
+}
+
 /*!
  * ioctl_init - V4L2 sensor interface handler for VIDIOC_INT_INIT
  * @s: pointer to standard V4L2 device structure
  */
 static int ioctl_init(struct v4l2_int_device *s)
 {
-	pr_debug("In mt9v111:ioctl_init\n");
+	int sensorid = 0;
+
+	sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+	if( sensorid < 0 )
+		return 0;
+
+	pr_debug("In mt9v111:ioctl_init for sensor %d\n",sensorid);
+
+	mt9v111_sensor_reset(sensorid);
+	mt9v111_ifp_reset(sensorid);
+	mt9v111_sensor_lib_datasheet(sensorid,mt9v111_device.coreReg, mt9v111_device.ifpReg);
 
 	return 0;
 }
@@ -873,19 +1128,146 @@ static int ioctl_init(struct v4l2_int_device *s)
  */
 static int ioctl_dev_init(struct v4l2_int_device *s)
 {
+	int sensorid = 0;
 	uint32_t clock_rate = MT9V111_MCLK;
 
+	sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+	if( sensorid < 0 )
+		return 0;
+
 	pr_debug("In mt9v111:ioctl_dev_init\n");
 
-	gpio_sensor_active();
+	set_mclk_rate(&clock_rate, 0);	// Both sensors use mclk0 on Digi ccwmx51
+
+	mt9v111_sensor_reset(sensorid);
+	mt9v111_ifp_reset(sensorid);
+	mt9v111_sensor_lib_datasheet(sensorid,mt9v111_device.coreReg, mt9v111_device.ifpReg);
 
-	set_mclk_rate(&clock_rate);
-	mt9v111_rate_cal(&reset_frame_rate, clock_rate);
-	mt9v111_sensor_lib(mt9v111_device.coreReg, mt9v111_device.ifpReg);
+	return 0;
+}
+
+/* list of image formats supported by sensor */
+static const struct v4l2_fmtdesc mt9v111_formats[] = {
+	{
+		.description = "RGB565",
+		.pixelformat = V4L2_PIX_FMT_RGB565,
+	},
+	{
+		.description = "YUV422 UYVY",
+		.pixelformat = V4L2_PIX_FMT_UYVY,
+	},
+};
+
+#define MT9V111_NUM_CAPTURE_FORMATS	ARRAY_SIZE(mt9v111_formats)
+
+static int ioctl_enum_fmt_cap(struct v4l2_int_device *s,
+				   struct v4l2_fmtdesc *fmt)
+{
+	int index = fmt->index;
+
+	switch (fmt->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		if (index >= MT9V111_NUM_CAPTURE_FORMATS)
+			return -EINVAL;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	fmt->flags = mt9v111_formats[index].flags;
+	strlcpy(fmt->description, mt9v111_formats[index].description,
+		sizeof(fmt->description));
+	fmt->pixelformat = mt9v111_formats[index].pixelformat;
 
 	return 0;
 }
 
+static int ioctl_s_fmt_cap(struct v4l2_int_device *s,
+			struct v4l2_format *f)
+{
+	unsigned short reg;
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+	struct sensor *sensor = s->priv;
+	/* s->priv points to mt9v111_data */
+
+	if( sensorid < 0 )
+		return -ENODEV;
+
+	/* Select IFP registers */
+	mt9v111_write_reg (sensorid,MT9V111S_ADDR_SPACE_SEL, 0x0001);
+
+	switch (f->fmt.pix.pixelformat) {
+		case V4L2_PIX_FMT_RGB565:
+			/*MT9V111I_OUTPUT_FORMAT_CTRL2*/
+			reg = mt9v111_read_reg (sensorid,MT9V111I_OUTPUT_FORMAT_CTRL2);
+			reg &= ~(0x3 << 6);
+			mt9v111_write_reg (sensorid,MT9V111I_OUTPUT_FORMAT_CTRL2, reg);
+
+			/* MT9V111I_FORMAT_CONTROL */
+			reg = mt9v111_read_reg(sensorid,MT9V111I_FORMAT_CONTROL);
+			reg |= 1 << 12;
+			mt9v111_write_reg(sensorid,MT9V111I_FORMAT_CONTROL, reg);
+			break;
+
+		case V4L2_PIX_FMT_YUV444:
+		case V4L2_PIX_FMT_UYVY:
+		case V4L2_PIX_FMT_YVU420:
+		case V4L2_PIX_FMT_YUYV:
+			/* MT9V111I_FORMAT_CONTROL */
+			reg = mt9v111_read_reg(sensorid,MT9V111I_FORMAT_CONTROL);
+			reg &= ~(1 << 12);
+			mt9v111_write_reg(sensorid,MT9V111I_FORMAT_CONTROL, reg);
+			break;
+
+		default:
+			return -EINVAL;
+	}
+
+	sensor->pix.width = f->fmt.pix.width;
+	sensor->pix.height = f->fmt.pix.height;
+	sensor->pix.sizeimage = f->fmt.pix.sizeimage;
+	sensor->pix.pixelformat = f->fmt.pix.pixelformat;
+	return 0;
+}
+
+static int ioctl_try_fmt_cap(struct v4l2_int_device *s, struct v4l2_format *f)
+{
+	int i;
+
+	if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	for( i=0 ; i < MT9V111_NUM_CAPTURE_FORMATS ; i++) {
+		if( f->fmt.pix.pixelformat == mt9v111_formats[i].pixelformat )
+			return 0;
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int ioctl_get_register(struct v4l2_int_device *s,struct v4l2_dbg_register * dreg)
+{
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , true, true);
+	dreg->val = mt9v111_read_reg (sensorid,dreg->reg);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , false, false);
+	return 0;
+}
+
+static int ioctl_set_register(struct v4l2_int_device *s,struct v4l2_dbg_register * dreg)
+{
+	int sensorid = mt9v111_id_from_name(((struct sensor *)s->priv)->v4l2_int_device->name);
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , true, true);
+	mt9v111_write_reg (sensorid,dreg->reg, dreg->val);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, 0 /* cam->csi */ , false, false);
+	return 0;
+}
+#endif
+
 /*!
  * This structure defines all the ioctls for this module and links them to the
  * enumeration.
@@ -910,16 +1292,23 @@ static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
 	/*!
 	 * VIDIOC_ENUM_FMT ioctl for the CAPTURE buffer type.
 	 */
-/*	{vidioc_int_enum_fmt_cap_num,
-				(v4l2_int_ioctl_func *) ioctl_enum_fmt_cap}, */
+	{vidioc_int_enum_fmt_cap_num,
+				(v4l2_int_ioctl_func *) ioctl_enum_fmt_cap},
+
 
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	{vidioc_int_g_register_num,
+				(v4l2_int_ioctl_func *) ioctl_get_register},
+	{vidioc_int_s_register_num,
+				(v4l2_int_ioctl_func *) ioctl_set_register},
+#endif
 	/*!
 	 * VIDIOC_TRY_FMT ioctl for the CAPTURE buffer type.
 	 * This ioctl is used to negotiate the image capture size and
 	 * pixel format without actually making it take effect.
 	 */
-/*	{vidioc_int_try_fmt_cap_num,
-				(v4l2_int_ioctl_func *) ioctl_try_fmt_cap}, */
+	{vidioc_int_try_fmt_cap_num,
+				(v4l2_int_ioctl_func *) ioctl_try_fmt_cap},
 
 	{vidioc_int_g_fmt_cap_num, (v4l2_int_ioctl_func *) ioctl_g_fmt_cap},
 
@@ -928,7 +1317,7 @@ static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
 	 * format, returns error code if format not supported or HW can't be
 	 * correctly configured.
 	 */
-/*	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap}, */
+	{vidioc_int_s_fmt_cap_num, (v4l2_int_ioctl_func *)ioctl_s_fmt_cap},
 
 	{vidioc_int_g_parm_num, (v4l2_int_ioctl_func *) ioctl_g_parm},
 	{vidioc_int_s_parm_num, (v4l2_int_ioctl_func *) ioctl_s_parm},
@@ -937,20 +1326,56 @@ static struct v4l2_int_ioctl_desc mt9v111_ioctl_desc[] = {
 	{vidioc_int_s_ctrl_num, (v4l2_int_ioctl_func *) ioctl_s_ctrl},
 };
 
-static struct v4l2_int_slave mt9v111_slave = {
-	.ioctls = mt9v111_ioctl_desc,
-	.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+static struct v4l2_int_slave mt9v111_slave[] = {
+	{
+		.ioctls = mt9v111_ioctl_desc,
+		.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+		.attach_to = "mxc_v4l2_cap_1",
+	},
+	{
+		.ioctls = mt9v111_ioctl_desc,
+		.num_ioctls = ARRAY_SIZE(mt9v111_ioctl_desc),
+		.attach_to = "mxc_v4l2_cap_2",
+	},
 };
 
-static struct v4l2_int_device mt9v111_int_device = {
-	.module = THIS_MODULE,
-	.name = "mt9v111",
-	.type = v4l2_int_type_slave,
-	.u = {
-		.slave = &mt9v111_slave,
+static struct v4l2_int_device mt9v111_int_device [] = {
+		{
+			.module = THIS_MODULE,
+			.type = v4l2_int_type_slave,
+			.u = {
+				.slave = &mt9v111_slave[0],
+				},
+		},
+		{
+			.module = THIS_MODULE,
+			.type = v4l2_int_type_slave,
+			.u = {
+				.slave = &mt9v111_slave[1],
+				},
 		},
 };
 
+static int mt9v111_read_id( int sensoridx )
+{
+	int sensorid = 0;
+	int ret = 0;
+
+	mt9v111_write_reg (sensoridx,MT9V111S_ADDR_SPACE_SEL, 0x0004);
+
+	sensorid = mt9v111_read_reg (sensoridx,MT9V111S_CHIP_VERSION);
+	if( sensorid == 0x823a )
+	{
+		printk(KERN_INFO" MT9V111 ID %x\n",sensorid);
+	}
+	else
+	{
+		printk(KERN_ERR" MT9V111 Could not detect sensor (read %x)\n",sensorid);
+		ret = -ENODEV;
+	}
+	return ret;
+}
+
 /*!
  * mt9v111 I2C probe function
  * Function set in i2c_driver struct.
@@ -962,32 +1387,56 @@ static int mt9v111_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	int retval;
+	int sensorid;
 
 	pr_debug("In mt9v111_probe  device id is %s\n", id->name);
 
+	sensorid = mt9v111_id_from_name(id->name);
+
+	if( sensorid < 0 )
+		return -ENODEV;
+
 	/* Set initial values for the sensor struct. */
-	memset(&mt9v111_data, 0, sizeof(mt9v111_data));
-	mt9v111_data.i2c_client = client;
+	memset(&mt9v111_data[sensorid], 0, sizeof(struct sensor));
+	mt9v111_data[sensorid].i2c_client = client;
 	pr_debug("   client name is %s\n", client->name);
-	mt9v111_data.pix.pixelformat = V4L2_PIX_FMT_UYVY;
-	mt9v111_data.pix.width = MT9V111_MAX_WIDTH;
-	mt9v111_data.pix.height = MT9V111_MAX_HEIGHT;
-	mt9v111_data.streamcap.capability = 0; /* No higher resolution or frame
-						* frame rate changes supported.
-						*/
-	mt9v111_data.streamcap.timeperframe.denominator = MT9V111_FRAME_RATE;
-	mt9v111_data.streamcap.timeperframe.numerator = 1;
+	mt9v111_data[sensorid].pix.pixelformat = V4L2_PIX_FMT_UYVY;
+	mt9v111_data[sensorid].pix.width = MT9V111_MAX_WIDTH;
+	mt9v111_data[sensorid].pix.height = MT9V111_MAX_HEIGHT;
+	mt9v111_data[sensorid].streamcap.capability = 0; /* No higher resolution or frame
+						* frame rate changes supported.*/
+	mt9v111_data[sensorid].streamcap.timeperframe.denominator = MT9V111_FRAME_RATE;
+	mt9v111_data[sensorid].streamcap.timeperframe.numerator = 1;
+
+	strcpy(mt9v111_int_device[sensorid].name,id->name);
+	pr_debug("   video device name is %s\n", mt9v111_data[sensorid].v4l2_int_device->name);
+	mt9v111_data[sensorid].v4l2_int_device = &mt9v111_int_device[sensorid];
+	mt9v111_int_device[sensorid].priv = &mt9v111_data[sensorid];
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, sensorid , true, true);
+
+	if( mt9v111_read_id(sensorid) != 0) {
+		printk(KERN_ERR"mt9v111_probe: No sensor found\n");
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, sensorid , false, false);
+		return -ENXIO;
+	}
 
-	mt9v111_int_device.priv = &mt9v111_data;
+#ifdef MT9V111_DEBUG
+	mt9v111_test_pattern(1);
+#endif
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, sensorid , false, false);
 
 	pr_debug("   type is %d (expect %d)\n",
-		mt9v111_int_device.type, v4l2_int_type_slave);
+		mt9v111_int_device[sensorid].type, v4l2_int_type_slave);
 	pr_debug("   num ioctls is %d\n",
-		mt9v111_int_device.u.slave->num_ioctls);
+		mt9v111_int_device[sensorid].u.slave->num_ioctls);
 
 	/* This function attaches this structure to the /dev/video0 device.
 	 * The pointer in priv points to the mt9v111_data structure here.*/
-	retval = v4l2_int_device_register(&mt9v111_int_device);
+	retval = v4l2_int_device_register(&mt9v111_int_device[sensorid]);
+	if( retval == 0 )
+		mt9v111_data[sensorid].used = 1;
 
 	return retval;
 }
@@ -998,9 +1447,14 @@ static int mt9v111_probe(struct i2c_client *client,
  */
 static int mt9v111_remove(struct i2c_client *client)
 {
+	int i;
+
 	pr_debug("In mt9v111_remove\n");
 
-	v4l2_int_device_unregister(&mt9v111_int_device);
+	for ( i=0 ; i < ARRAY_SIZE(mt9v111_int_device) ; i++ ) {
+		if( mt9v111_data[i].used )
+			v4l2_int_device_unregister(&mt9v111_int_device[i]);
+	}
 	return 0;
 }
 
@@ -1033,13 +1487,13 @@ static __init int mt9v111_init(void)
 	memset(mt9v111_device.ifpReg, 0, sizeof(mt9v111_IFPReg));
 
 	/* Set contents of the just created structures. */
-	mt9v111_config();
+	mt9v111_config_datasheet();
 
 	/* Tells the i2c driver what functions to call for this driver. */
 	err = i2c_add_driver(&mt9v111_i2c_driver);
 	if (err != 0)
 		pr_err("%s:driver registration failed, error=%d \n",
-		       __func__, err);
+			   __func__, err);
 
 	return err;
 }
@@ -1055,7 +1509,6 @@ static void __exit mt9v111_clean(void)
 	pr_debug("In mt9v111_clean()\n");
 
 	i2c_del_driver(&mt9v111_i2c_driver);
-	gpio_sensor_inactive();
 
 	if (mt9v111_device.coreReg) {
 		kfree(mt9v111_device.coreReg);
diff --git a/drivers/media/video/mxc/capture/mt9v111.h b/drivers/media/video/mxc/capture/mt9v111.h
index cf38cec4757c..ba91a722a076 100644
--- a/drivers/media/video/mxc/capture/mt9v111.h
+++ b/drivers/media/video/mxc/capture/mt9v111.h
@@ -111,11 +111,14 @@
 #define MT9V111I_GAMMA_KNEE_Y90           0x57
 #define MT9V111I_GAMMA_VALUE_Y0           0x58
 #define MT9V111I_SHUTTER_60               0x59
+#define MT9V111I_AUTO_EXPOSURE_17         0x5A
 #define MT9V111I_SEARCH_FLICK_60          0x5c
+#define MT9V111I_RESERVED93	    	      0x5d
 #define MT9V111I_RATIO_IMAGE_GAIN_BASE    0x5e
 #define MT9V111I_RATIO_IMAGE_GAIN_DELTA   0x5f
 #define MT9V111I_SIGN_VALUE_REG5F         0x60
 #define MT9V111I_AE_GAIN                  0x62
+#define MT9V111I_RESERVED100              0x64
 #define MT9V111I_MAX_GAIN_AE              0x67
 #define MT9V111I_LENS_CORRECT_CTRL        0x80
 #define MT9V111I_SHADING_PARAMETER1       0x81
@@ -173,6 +176,7 @@
 #define MT9V111S_ROW_START_IN_ZOOM        0x13
 #define MT9V111S_DIGITAL_ZOOM             0x1e
 #define MT9V111S_READ_MODE                0x20
+#define MT9V111S_RESERVED33               0x21
 #define MT9V111S_DAC_CTRL                 0x27
 #define MT9V111S_GREEN1_GAIN              0x2b
 #define MT9V111S_BLUE_GAIN                0x2c
@@ -278,6 +282,7 @@ typedef struct {
 	u32 rowNoiseControl;
 	u32 darkTargetwNC;
 	u32 testData;		/*!< test mode */
+	u32 reserved33;
 	u32 globalGain;
 	u32 chipVersion;
 	u32 darkTargetwoNC;
@@ -375,11 +380,14 @@ typedef struct {
 	u32 gammaKneeY90;	/*!< Gamma knee points Y9 and Y10 */
 	u32 gammaKneeY0;	/*!< Gamma knee point Y0 */
 	u32 shutter_width_60;
+	u32 auto_exposure_17;
 	u32 search_flicker_60;
+	u32 reserved93;
 	u32 ratioImageGainBase;
 	u32 ratioImageGainDelta;
 	u32 signValueReg5F;
 	u32 aeGain;
+	u32 reserved100;
 	u32 maxGainAE;
 	u32 lensCorrectCtrl;
 	u32 shadingParameter1;	/*!< Shade Parameters */
diff --git a/drivers/media/video/mxc/capture/mx27_prpsw.c b/drivers/media/video/mxc/capture/mx27_prpsw.c
index ce7db16913ec..eca200a580f2 100644
--- a/drivers/media/video/mxc/capture/mx27_prpsw.c
+++ b/drivers/media/video/mxc/capture/mx27_prpsw.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -702,7 +702,7 @@ static int prp_still_start(void *private)
 	cam_data *cam = (cam_data *) private;
 
 	g_still_on = 1;
-	g_prp_cfg.ch2_ptr = (unsigned int)cam->still_buf;
+	g_prp_cfg.ch2_ptr = (unsigned int)cam->still_buf[0];
 	g_prp_cfg.ch2_ptr2 = 0;
 
 	if (prp_v4l2_cfg(&g_prp_cfg, cam))
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
index 30ad533b0ebd..be9a988aff2f 100644
--- a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
@@ -18,6 +18,8 @@
  *
  * @ingroup MXC_V4L2_CAPTURE
  */
+
+
 #include <linux/version.h>
 #include <linux/module.h>
 #include <linux/init.h>
@@ -37,9 +39,9 @@
 #include <media/v4l2-int-device.h>
 #include "mxc_v4l2_capture.h"
 #include "ipu_prp_sw.h"
+#include "asm/delay.h"
 
 static int video_nr = -1;
-static cam_data *g_cam;
 
 /*! This data is used for the output to the display. */
 #define MXC_V4L2_CAPTURE_NUM_OUTPUTS	3
@@ -74,21 +76,23 @@ static struct v4l2_output mxc_capture_outputs[MXC_V4L2_CAPTURE_NUM_OUTPUTS] = {
 static struct v4l2_input mxc_capture_inputs[MXC_V4L2_CAPTURE_NUM_INPUTS] = {
 	{
 	 .index = 0,
-	 .name = "CSI IC MEM",
+	 .name = "CSI MEM",
 	 .type = V4L2_INPUT_TYPE_CAMERA,
 	 .audioset = 0,
 	 .tuner = 0,
 	 .std = V4L2_STD_UNKNOWN,
-	 .status = 0,
+	 .status = V4L2_IN_ST_NO_POWER,
 	 },
 	{
 	 .index = 1,
+// AG: CSI IC MEM works but has problems
+//	 .name = "CSI IC MEM",
 	 .name = "CSI MEM",
 	 .type = V4L2_INPUT_TYPE_CAMERA,
 	 .audioset = 0,
 	 .tuner = 0,
 	 .std = V4L2_STD_UNKNOWN,
-	 .status = V4L2_IN_ST_NO_POWER,
+	 .status = 0,
 	 },
 };
 
@@ -118,6 +122,7 @@ typedef struct {
 	u16 active_left;	/*!< Active left. */
 } video_fmt_t;
 
+#if 0
 /*!
  * Description of video formats supported.
  *
@@ -128,37 +133,39 @@ static video_fmt_t video_fmts[] = {
 	{			/*! NTSC */
 	 .v4l2_id = V4L2_STD_NTSC,
 	 .name = "NTSC",
-	 .raw_width = 720 - 1,		/* SENS_FRM_WIDTH */
-	 .raw_height = 288 - 1,		/* SENS_FRM_HEIGHT */
-	 .active_width = 720,		/* ACT_FRM_WIDTH plus 1 */
-	 .active_height = (480 / 2),	/* ACT_FRM_HEIGHT plus 1 */
-	 .active_top = 12,
+	 .raw_width = 720,		/* SENS_FRM_WIDTH */
+	 .raw_height = 525,		/* SENS_FRM_HEIGHT */
+	 .active_width = 720,		/* ACT_FRM_WIDTH */
+	 .active_height = 240,		/* ACT_FRM_HEIGHT */
+	 .active_top = 0,
 	 .active_left = 0,
 	 },
 	{			/*! (B, G, H, I, N) PAL */
 	 .v4l2_id = V4L2_STD_PAL,
 	 .name = "PAL",
-	 .raw_width = 720 - 1,
-	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .raw_width = 720,
+	 .raw_height = 625,
 	 .active_width = 720,
-	 .active_height = (576 / 2),
+	 .active_height = 288,
 	 .active_top = 0,
 	 .active_left = 0,
 	 },
 	{			/*! Unlocked standard */
 	 .v4l2_id = V4L2_STD_ALL,
 	 .name = "Autodetect",
-	 .raw_width = 720 - 1,
-	 .raw_height = (576 / 2) + 24 * 2 - 1,
+	 .raw_width = 720,
+	 .raw_height = 625,
 	 .active_width = 720,
-	 .active_height = (576 / 2),
+	 .active_height = 288,
 	 .active_top = 0,
 	 .active_left = 0,
 	 },
 };
 
+
 /*!* Standard index of TV. */
 static video_fmt_idx video_index = TV_NOT_LOCKED;
+#endif
 
 static int mxc_v4l2_master_attach(struct v4l2_int_device *slave);
 static void mxc_v4l2_master_detach(struct v4l2_int_device *slave);
@@ -172,15 +179,27 @@ static struct v4l2_int_master mxc_v4l2_master = {
 	.detach = mxc_v4l2_master_detach,
 };
 
-static struct v4l2_int_device mxc_v4l2_int_device = {
+static struct v4l2_int_device mxc_v4l2_int_device [] = {
+	{
+	.module = THIS_MODULE,
+	.name = "mxc_v4l2_cap_1",
+	.type = v4l2_int_type_master,
+	.u = {
+		.master = &mxc_v4l2_master,
+		},
+	},
+	{
 	.module = THIS_MODULE,
-	.name = "mxc_v4l2_cap",
+	.name = "mxc_v4l2_cap_2",
 	.type = v4l2_int_type_master,
 	.u = {
 		.master = &mxc_v4l2_master,
 		},
+	},
 };
 
+static cam_data *g_cam[ARRAY_SIZE(mxc_v4l2_int_device)];
+
 /***************************************************************************
  * Functions for handling Frame buffers.
  **************************************************************************/
@@ -260,6 +279,7 @@ static int mxc_allocate_frame_buf(cam_data *cam, int count)
 static void mxc_free_frames(cam_data *cam)
 {
 	int i;
+	unsigned long lock_flags;
 
 	pr_debug("In MVC:mxc_free_frames\n");
 
@@ -269,9 +289,11 @@ static void mxc_free_frames(cam_data *cam)
 
 	cam->enc_counter = 0;
 	cam->skip_frame = 0;
+	spin_lock_irqsave(&cam->dqueue_int_lock, lock_flags);
 	INIT_LIST_HEAD(&cam->ready_q);
 	INIT_LIST_HEAD(&cam->working_q);
 	INIT_LIST_HEAD(&cam->done_q);
+	spin_unlock_irqrestore(&cam->dqueue_int_lock, lock_flags);
 }
 
 /*!
@@ -331,8 +353,7 @@ static int mxc_streamon(cam_data *cam)
 {
 	struct mxc_v4l_frame *frame;
 	int err = 0;
-
-	pr_debug("In MVC:mxc_streamon\n");
+	unsigned long lock_flags;
 
 	if (NULL == cam) {
 		pr_err("ERROR! cam parameter is NULL\n");
@@ -363,23 +384,27 @@ static int mxc_streamon(cam_data *cam)
 		}
 	}
 
+	spin_lock_irqsave(&cam->queue_int_lock, lock_flags);
 	cam->ping_pong_csi = 0;
 	if (cam->enc_update_eba) {
-		frame =
-		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
-		list_del(cam->ready_q.next);
-		list_add_tail(&frame->queue, &cam->working_q);
-		err = cam->enc_update_eba(frame->buffer.m.offset,
-					  &cam->ping_pong_csi);
 
 		frame =
 		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
 		list_del(cam->ready_q.next);
 		list_add_tail(&frame->queue, &cam->working_q);
-		err |= cam->enc_update_eba(frame->buffer.m.offset,
-					   &cam->ping_pong_csi);
+		err = cam->enc_update_eba(cam->csi,frame->buffer.m.offset,
+					  &cam->ping_pong_csi);
+		if (!list_empty(&cam->ready_q)) {
+			frame =
+				list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
+			list_del(cam->ready_q.next);
+			list_add_tail(&frame->queue, &cam->working_q);
+			err |= cam->enc_update_eba(cam->csi,frame->buffer.m.offset,
+						   &cam->ping_pong_csi);
+		}
+		cam->capture_on = true;
 	} else {
-		return -EINVAL;
+		err = -EINVAL;
 	}
 
 	if (cam->overlay_on == true)
@@ -387,11 +412,9 @@ static int mxc_streamon(cam_data *cam)
 
 	if (cam->enc_enable_csi) {
 		err = cam->enc_enable_csi(cam);
-		if (err != 0)
-			return err;
 	}
 
-	cam->capture_on = true;
+	spin_unlock_irqrestore(&cam->queue_int_lock, lock_flags);
 
 	return err;
 }
@@ -543,8 +566,6 @@ static int start_preview(cam_data *cam)
 {
 	int err = 0;
 
-	pr_debug("MVC: start_preview\n");
-
 #if defined(CONFIG_MXC_IPU_PRP_VF_SDC) || defined(CONFIG_MXC_IPU_PRP_VF_SDC_MODULE)
 	pr_debug("   This is an SDC display\n");
 	if (cam->output == 0 || cam->output == 2) {
@@ -602,8 +623,6 @@ static int stop_preview(cam_data *cam)
 {
 	int err = 0;
 
-	pr_debug("MVC: stop preview\n");
-
 #if defined(CONFIG_MXC_IPU_PRP_VF_ADC) || defined(CONFIG_MXC_IPU_PRP_VF_ADC_MODULE)
 	if (cam->output == 1) {
 		err = prp_vf_adc_deselect(cam);
@@ -674,6 +693,29 @@ static int mxc_v4l2_g_fmt(cam_data *cam, struct v4l2_format *f)
 		 __func__,
 		 cam->crop_current.width, cam->crop_current.height);
 
+	retval = vidioc_int_g_fmt_cap(cam->sensor,f);
+	return retval;
+}
+
+/*!
+ * V4L2 - mxc_v4l2_enum_fmt function
+ *
+ * @param cam         structure cam_data *
+ *
+ * @param f           structure v4l2_fmtdesc *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_enum_fmt(cam_data *cam, struct v4l2_fmtdesc *f)
+{
+	int retval = 0;
+
+	pr_debug("In MVC: mxc_v4l2_enum_fmt\n");
+
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	retval = vidioc_int_enum_fmt_cap(cam->sensor,f);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+
 	return retval;
 }
 
@@ -709,10 +751,14 @@ static int mxc_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
 		 * for CSI MEM input mode.
 		 */
 		if (strcmp(mxc_capture_inputs[cam->current_input].name,
-			   "CSI MEM") == 0) {
+			   "CSI MEM") == 0 || strcmp(mxc_capture_inputs[cam->current_input].name,
+					   "CSI IC MEM") == 0) {
 			f->fmt.pix.width = cam->crop_current.width;
 			f->fmt.pix.height = cam->crop_current.height;
 		}
+		else {
+			printk("Error no match %s\n",mxc_capture_inputs[cam->current_input].name);
+		}
 
 		if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
 			height = &f->fmt.pix.width;
@@ -811,6 +857,13 @@ static int mxc_v4l2_s_fmt(cam_data *cam, struct v4l2_format *f)
 				break;
 			}
 		}
+
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
+				       true, true);
+		vidioc_int_s_fmt_cap(cam->sensor, f);
+		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
+				       false, false);
+
 		break;
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 		pr_debug("   type=V4L2_BUF_TYPE_VIDEO_OVERLAY\n");
@@ -1057,63 +1110,10 @@ static int mxc_v4l2_s_ctrl(cam_data *cam, struct v4l2_control *c)
 	return ret;
 }
 
-/*!
- * V4L2 - mxc_v4l2_s_param function
- * Allows setting of capturemode and frame rate.
- *
- * @param cam         structure cam_data *
- * @param parm        structure v4l2_streamparm *
- *
- * @return  status    0 success, EINVAL failed
- */
-static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
-{
-	struct v4l2_ifparm ifparm;
+static int 	mxc_v4l2_init_csi( cam_data *cam ) {
 	struct v4l2_format cam_fmt;
-	struct v4l2_streamparm currentparm;
 	ipu_csi_signal_cfg_t csi_param;
-	int err = 0;
-
-	pr_debug("In mxc_v4l2_s_param\n");
-
-	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		pr_err(KERN_ERR "mxc_v4l2_s_param invalid type\n");
-		return -EINVAL;
-	}
-
-	/* Stop the viewfinder */
-	if (cam->overlay_on == true) {
-		stop_preview(cam);
-	}
-
-	currentparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-
-	/* First check that this device can support the changes requested. */
-	err = vidioc_int_g_parm(cam->sensor, &currentparm);
-	if (err) {
-		pr_err("%s: vidioc_int_g_parm returned an error %d\n",
-			__func__, err);
-		goto exit;
-	}
-
-	pr_debug("   Current capabilities are %x\n",
-			currentparm.parm.capture.capability);
-	pr_debug("   Current capturemode is %d  change to %d\n",
-			currentparm.parm.capture.capturemode,
-			parm->parm.capture.capturemode);
-	pr_debug("   Current framerate is %d  change to %d\n",
-			currentparm.parm.capture.timeperframe.denominator,
-			parm->parm.capture.timeperframe.denominator);
-
-	/* This will change any camera settings needed. */
-	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
-	err = vidioc_int_s_parm(cam->sensor, parm);
-	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
-	if (err) {
-		pr_err("%s: vidioc_int_s_parm returned an error %d\n",
-			__func__, err);
-		goto exit;
-	}
+	struct v4l2_ifparm ifparm;
 
 	/* If resolution changed, need to re-program the CSI */
 	/* Get new values. */
@@ -1131,13 +1131,13 @@ static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
 	csi_param.force_eof = 0;
 	csi_param.data_en_pol = 0;
 	csi_param.data_fmt = 0;
-	csi_param.csi = 0;
+	csi_param.csi = cam->csi;
 	csi_param.mclk = 0;
 
 	/* This may not work on other platforms. Check when adding a new one.*/
 	pr_debug("   clock_curr=mclk=%d\n", ifparm.u.bt656.clock_curr);
 	if (ifparm.u.bt656.clock_curr == 0) {
-		csi_param.clk_mode = IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE;
+		csi_param.clk_mode = IPU_CSI_CLK_MODE_CCIR656_INTERLACED;
 	} else {
 		csi_param.clk_mode = IPU_CSI_CLK_MODE_GATED_CLK;
 	}
@@ -1189,7 +1189,63 @@ static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
 	ipu_csi_init_interface(cam->crop_bounds.width,
 			       cam->crop_bounds.height,
 			       cam_fmt.fmt.pix.pixelformat, csi_param);
+	return 0;
+}
 
+/*!
+ * V4L2 - mxc_v4l2_s_param function
+ * Allows setting of capturemode and frame rate.
+ *
+ * @param cam         structure cam_data *
+ * @param parm        structure v4l2_streamparm *
+ *
+ * @return  status    0 success, EINVAL failed
+ */
+static int mxc_v4l2_s_param(cam_data *cam, struct v4l2_streamparm *parm)
+{
+	struct v4l2_streamparm currentparm;
+	int err = 0;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+		pr_err(KERN_ERR "mxc_v4l2_s_param invalid type\n");
+		return -EINVAL;
+	}
+
+	/* Stop the viewfinder */
+	if (cam->overlay_on == true) {
+		stop_preview(cam);
+	}
+
+	currentparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* First check that this device can support the changes requested. */
+	err = vidioc_int_g_parm(cam->sensor, &currentparm);
+	if (err) {
+		pr_err("%s: vidioc_int_g_parm returned an error %d\n",
+			__func__, err);
+		goto exit;
+	}
+
+	pr_debug("   Current capabilities are %x\n",
+			currentparm.parm.capture.capability);
+	pr_debug("   Current capturemode is %d  change to %d\n",
+			currentparm.parm.capture.capturemode,
+			parm->parm.capture.capturemode);
+	pr_debug("   Current framerate is %d  change to %d\n",
+			currentparm.parm.capture.timeperframe.denominator,
+			parm->parm.capture.timeperframe.denominator);
+
+	/* This will change any camera settings needed. */
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	err = vidioc_int_s_parm(cam->sensor, parm);
+	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
+	if (err) {
+		pr_err("%s: vidioc_int_s_parm returned an error %d\n",
+			__func__, err);
+		goto exit;
+	}
+
+	err = mxc_v4l2_init_csi(cam);
 
 exit:
 	if (cam->overlay_on == true)
@@ -1198,6 +1254,7 @@ exit:
 	return err;
 }
 
+#if 0
 /*!
  * V4L2 - mxc_v4l2_s_std function
  *
@@ -1287,6 +1344,7 @@ static int mxc_v4l2_g_std(cam_data *cam, v4l2_std_id *e)
 
 	return 0;
 }
+#endif
 
 /*!
  * Dequeue one V4L capture buffer
@@ -1372,6 +1430,11 @@ static int mxc_v4l_open(struct file *file)
 		return -EBADF;
 	}
 
+	if(!cam->sensor) {
+		pr_err("ERROR: v4l2 capture: Unattached sensor!\n");
+		return -EBADF;
+	}
+
 	down(&cam->busy_lock);
 	err = 0;
 	if (signal_pending(current))
@@ -1411,12 +1474,9 @@ static int mxc_v4l_open(struct file *file)
 		csi_param.force_eof = 0;
 		csi_param.data_en_pol = 0;
 		csi_param.mclk = ifparm.u.bt656.clock_curr;
-
+		csi_param.ext_vsync = ifparm.u.bt656.bt_sync_correct;
 		csi_param.pixclk_pol = ifparm.u.bt656.latch_clk_inv;
 
-		/* Once we handle multiple inputs this will need to change. */
-		csi_param.csi = 0;
-
 		if (ifparm.u.bt656.mode
 				== V4L2_IF_TYPE_BT656_MODE_NOBT_8BIT)
 			csi_param.data_width = IPU_CSI_DATA_WIDTH_8;
@@ -1464,11 +1524,13 @@ static int mxc_v4l_open(struct file *file)
 			__func__,
 			cam->crop_current.width, cam->crop_current.height);
 
+		udelay(100);
+
 		csi_param.data_fmt = cam_fmt.fmt.pix.pixelformat;
 		pr_debug("On Open: Input to ipu size is %d x %d\n",
 				cam_fmt.fmt.pix.width, cam_fmt.fmt.pix.height);
 		ipu_csi_set_window_size(cam->crop_current.width,
-					cam->crop_current.width,
+					cam->crop_current.height,
 					cam->csi);
 		ipu_csi_set_window_pos(cam->crop_current.left,
 					cam->crop_current.top,
@@ -1478,12 +1540,16 @@ static int mxc_v4l_open(struct file *file)
 					cam_fmt.fmt.pix.pixelformat,
 					csi_param);
 
+		udelay(100);
+
 		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
 				       true, true);
 		vidioc_int_init(cam->sensor);
 
 		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi,
 				       false, false);
+
+		udelay(100);
 }
 
 	file->private_data = dev;
@@ -1519,7 +1585,7 @@ static int mxc_v4l_close(struct file *file)
 		err = stop_preview(cam);
 		cam->overlay_on = false;
 	}
-	if (cam->capture_pid == current->pid) {
+	if (cam->capture_pid == current->tgid) {
 		err |= mxc_streamoff(cam);
 		wake_up_interruptible(&cam->enc_queue);
 	}
@@ -1570,7 +1636,7 @@ static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
 			    loff_t *ppos)
 {
 	int err = 0;
-	u8 *v_address;
+	u8 *v_address[2];
 	struct video_device *dev = video_devdata(file);
 	cam_data *cam = video_get_drvdata(dev);
 
@@ -1581,11 +1647,17 @@ static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
 	if (cam->overlay_on == true)
 		stop_preview(cam);
 
-	v_address = dma_alloc_coherent(0,
+	v_address[0] = dma_alloc_coherent(0,
 				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
-				       &cam->still_buf, GFP_DMA | GFP_KERNEL);
+				       &cam->still_buf[0],
+				       GFP_DMA | GFP_KERNEL);
+
+	v_address[1] = dma_alloc_coherent(0,
+				       PAGE_ALIGN(cam->v2f.fmt.pix.sizeimage),
+				       &cam->still_buf[1],
+				       GFP_DMA | GFP_KERNEL);
 
-	if (!v_address) {
+	if (!v_address[0] || !v_address[1]) {
 		err = -ENOBUFS;
 		goto exit0;
 	}
@@ -1593,14 +1665,14 @@ static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
 	err = prp_still_select(cam);
 	if (err != 0) {
 		err = -EIO;
-		goto exit1;
+		goto exit0;
 	}
 
 	cam->still_counter = 0;
 	err = cam->csi_start(cam);
 	if (err != 0) {
 		err = -EIO;
-		goto exit2;
+		goto exit1;
 	}
 
 	if (!wait_event_interruptible_timeout(cam->still_queue,
@@ -1609,19 +1681,23 @@ static ssize_t mxc_v4l_read(struct file *file, char *buf, size_t count,
 		pr_err("ERROR: v4l2 capture: mxc_v4l_read timeout counter %x\n",
 		       cam->still_counter);
 		err = -ETIME;
-		goto exit2;
+		goto exit1;
 	}
-	err = copy_to_user(buf, v_address, cam->v2f.fmt.pix.sizeimage);
-
-      exit2:
-	prp_still_deselect(cam);
+	err = copy_to_user(buf, v_address[1], cam->v2f.fmt.pix.sizeimage);
 
       exit1:
-	dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address,
-			  cam->still_buf);
-	cam->still_buf = 0;
+	prp_still_deselect(cam);
 
       exit0:
+	if (v_address[0] != 0)
+		dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address[0],
+				  cam->still_buf[0]);
+	if (v_address[1] != 0)
+		dma_free_coherent(0, cam->v2f.fmt.pix.sizeimage, v_address[1],
+				  cam->still_buf[1]);
+
+	cam->still_buf[0] = cam->still_buf[1] = 0;
+
 	if (cam->overlay_on == true) {
 		start_preview(cam);
 	}
@@ -1661,6 +1737,21 @@ static long mxc_v4l_do_ioctl(struct file *file,
 		return -EBUSY;
 
 	switch (ioctlnr) {
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	case VIDIOC_DBG_S_REGISTER: {
+		struct v4l2_dbg_register * dreg = arg;
+		vidioc_int_s_register(cam->sensor,dreg);
+		break;
+	}
+
+	case VIDIOC_DBG_G_REGISTER: {
+		struct v4l2_dbg_register * dreg = arg;
+		vidioc_int_g_register(cam->sensor,dreg);
+		break;
+	}
+#endif
+
 	/*!
 	 * V4l2 VIDIOC_QUERYCAP ioctl
 	 */
@@ -1695,6 +1786,14 @@ static long mxc_v4l_do_ioctl(struct file *file,
 		struct v4l2_format *sf = arg;
 		pr_debug("   case VIDIOC_S_FMT\n");
 		retval = mxc_v4l2_s_fmt(cam, sf);
+		mxc_v4l2_init_csi(cam);
+		break;
+	}
+
+	case VIDIOC_ENUM_FMT: {
+		struct v4l2_fmtdesc *fd = arg;
+		pr_debug("   case VIDIOC_ENUM_FMT\n");
+		retval = mxc_v4l2_enum_fmt(cam, fd);
 		break;
 	}
 
@@ -1775,9 +1874,8 @@ static long mxc_v4l_do_ioctl(struct file *file,
 				if (cam->skip_frame > 0) {
 					list_add_tail(&cam->frame[index].queue,
 						      &cam->working_q);
-
 					retval =
-					   cam->enc_update_eba(cam->
+					   cam->enc_update_eba(cam->csi,cam->
 							       frame[index].
 							       buffer.m.offset,
 							       &cam->
@@ -1999,28 +2097,6 @@ static long mxc_v4l_do_ioctl(struct file *file,
 		break;
 	}
 
-	/* linux v4l2 bug, kernel c0485619 user c0405619 */
-	case VIDIOC_ENUMSTD: {
-		struct v4l2_standard *e = arg;
-		pr_debug("   case VIDIOC_ENUMSTD\n");
-		*e = cam->standard;
-		break;
-	}
-
-	case VIDIOC_G_STD: {
-		v4l2_std_id *e = arg;
-		pr_debug("   case VIDIOC_G_STD\n");
-		retval = mxc_v4l2_g_std(cam, e);
-		break;
-	}
-
-	case VIDIOC_S_STD: {
-		v4l2_std_id *e = arg;
-		pr_debug("   case VIDIOC_S_STD\n");
-		retval = mxc_v4l2_s_std(cam, *e);
-
-		break;
-	}
 
 	case VIDIOC_ENUMOUTPUT: {
 		struct v4l2_output *output = arg;
@@ -2109,8 +2185,13 @@ static long mxc_v4l_do_ioctl(struct file *file,
 		break;
 	}
 
-	case VIDIOC_ENUM_FMT:
-	case VIDIOC_TRY_FMT:
+	case VIDIOC_TRY_FMT: {
+		struct v4l2_format * f = arg;
+		pr_debug("   case VIDIOC_TRY_FMT\n");
+		retval = vidioc_int_try_fmt_cap(cam->sensor,f);
+		break;
+	}
+
 	case VIDIOC_QUERYCTRL:
 	case VIDIOC_G_TUNER:
 	case VIDIOC_S_TUNER:
@@ -2163,7 +2244,7 @@ static int mxc_mmap(struct file *file, struct vm_area_struct *vma)
 		return -EINTR;
 
 	size = vma->vm_end - vma->vm_start;
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
 
 	if (remap_pfn_range(vma, vma->vm_start,
 			    vma->vm_pgoff, size, vma->vm_page_prot)) {
@@ -2236,12 +2317,21 @@ static void camera_platform_release(struct device *device)
 }
 
 /*! Device Definition for Mt9v111 devices */
-static struct platform_device mxc_v4l2_devices = {
-	.name = "mxc_v4l2",
-	.dev = {
-		.release = camera_platform_release,
-		},
-	.id = 0,
+static struct platform_device mxc_v4l2_devices[] = {
+	{
+		.name = "mxc_v4l2_1",
+		.dev = {
+			.release = camera_platform_release,
+			},
+		.id = 0,
+	},
+	{
+		.name = "mxc_v4l2_2",
+		.dev = {
+			.release = camera_platform_release,
+			},
+		.id = 1,
+	}
 };
 
 /*!
@@ -2302,7 +2392,7 @@ static void camera_callback(u32 mask, void *dev)
 							 struct mxc_v4l_frame,
 							 queue);
 
-				if (cam->enc_update_eba(
+				if (cam->enc_update_eba(cam->csi,
 						ready_frame->buffer.m.offset,
 						&cam->ping_pong_csi) == 0) {
 					list_del(cam->ready_q.next);
@@ -2354,7 +2444,7 @@ static void camera_callback(u32 mask, void *dev)
 			ready_frame = list_entry(cam->ready_q.next,
 						 struct mxc_v4l_frame,
 						 queue);
-			if (cam->enc_update_eba(ready_frame->buffer.m.offset,
+			if (cam->enc_update_eba(cam->csi,ready_frame->buffer.m.offset,
 						&cam->ping_pong_csi) == 0) {
 				list_del(cam->ready_q.next);
 				list_add_tail(&ready_frame->queue,
@@ -2362,7 +2452,7 @@ static void camera_callback(u32 mask, void *dev)
 			} else
 				return;
 		} else {
-			if (cam->enc_update_eba(
+			if (cam->enc_update_eba(cam->csi,
 				cam->dummy_frame.buffer.m.offset,
 				&cam->ping_pong_csi) == -EACCES)
 				return;
@@ -2379,13 +2469,15 @@ static void camera_callback(u32 mask, void *dev)
  *
  * @return status  0 Success
  */
-static void init_camera_struct(cam_data *cam)
+static void init_camera_struct(cam_data *cam,unsigned int csi)
 {
-	pr_debug("In MVC: init_camera_struct\n");
+	pr_debug("In MVC: init_camera_struct for csi %d\n",csi);
 
 	/* Default everything to 0 */
 	memset(cam, 0, sizeof(cam_data));
 
+	cam->csi = csi;
+
 	init_MUTEX(&cam->param_lock);
 	init_MUTEX(&cam->busy_lock);
 
@@ -2396,7 +2488,7 @@ static void init_camera_struct(cam_data *cam)
 	*(cam->video_dev) = mxc_v4l_template;
 
 	video_set_drvdata(cam->video_dev, cam);
-	dev_set_drvdata(&mxc_v4l2_devices.dev, (void *)cam);
+	dev_set_drvdata(&mxc_v4l2_devices[csi].dev, (void *)cam);
 	cam->video_dev->minor = -1;
 
 	init_waitqueue_head(&cam->enc_queue);
@@ -2428,6 +2520,8 @@ static void init_camera_struct(cam_data *cam)
 	cam->skip_frame = 0;
 	cam->v4l2_fb.flags = V4L2_FBUF_FLAG_OVERLAY;
 
+	cam->current_input = cam->csi;
+
 	cam->v2f.fmt.pix.sizeimage = 352 * 288 * 3 / 2;
 	cam->v2f.fmt.pix.bytesperline = 288 * 3 / 2;
 	cam->v2f.fmt.pix.width = 288;
@@ -2438,9 +2532,6 @@ static void init_camera_struct(cam_data *cam)
 	cam->win.w.left = 0;
 	cam->win.w.top = 0;
 
-	cam->csi = 0;  /* Need to determine how to set this correctly with
-			* multiple video input devices. */
-
 	cam->enc_callback = camera_callback;
 	init_waitqueue_head(&cam->power_queue);
 	spin_lock_init(&cam->queue_int_lock);
@@ -2460,11 +2551,12 @@ static u8 camera_power(cam_data *cam, bool cameraOn)
 {
 	pr_debug("In MVC:camera_power on=%d\n", cameraOn);
 
+	if( !cam->open_count )
+		return 0;
+
 	if (cameraOn == true) {
-		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
 		vidioc_int_s_power(cam->sensor, 1);
 	} else {
-		ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
 		vidioc_int_s_power(cam->sensor, 0);
 	}
 	return 0;
@@ -2491,15 +2583,15 @@ static int mxc_v4l2_suspend(struct platform_device *pdev, pm_message_t state)
 		return -1;
 	}
 
+	if (!cam->open_count) {
+		return 0;
+	}
+
 	cam->low_power = true;
 
 	if (cam->overlay_on == true)
 		stop_preview(cam);
-	if ((cam->capture_on == true) && cam->enc_disable) {
-		cam->enc_disable(cam);
-	}
 	camera_power(cam, false);
-
 	return 0;
 }
 
@@ -2522,14 +2614,19 @@ static int mxc_v4l2_resume(struct platform_device *pdev)
 		return -1;
 	}
 
+	if( !cam->open_count )
+		return 0;
+
 	cam->low_power = false;
 	wake_up_interruptible(&cam->power_queue);
+
 	camera_power(cam, true);
 
 	if (cam->overlay_on == true)
 		start_preview(cam);
+
 	if (cam->capture_on == true)
-		mxc_streamon(cam);
+	    mxc_streamon(cam);
 
 	return 0;
 }
@@ -2537,15 +2634,27 @@ static int mxc_v4l2_resume(struct platform_device *pdev)
 /*!
  * This structure contains pointers to the power management callback functions.
  */
-static struct platform_driver mxc_v4l2_driver = {
-	.driver = {
-		   .name = "mxc_v4l2",
-		   },
-	.probe = NULL,
-	.remove = NULL,
-	.suspend = mxc_v4l2_suspend,
-	.resume = mxc_v4l2_resume,
-	.shutdown = NULL,
+static struct platform_driver mxc_v4l2_driver[] = {
+	{
+		.driver = {
+			   .name = "mxc_v4l2_1",
+			   },
+		.probe = NULL,
+		.remove = NULL,
+		.suspend = mxc_v4l2_suspend,
+		.resume = mxc_v4l2_resume,
+		.shutdown = NULL,
+	},
+	{
+		.driver = {
+			   .name = "mxc_v4l2_2",
+			   },
+		.probe = NULL,
+		.remove = NULL,
+		.suspend = mxc_v4l2_suspend,
+		.resume = mxc_v4l2_resume,
+		.shutdown = NULL,
+	},
 };
 
 /*!
@@ -2567,6 +2676,7 @@ static int mxc_v4l2_master_attach(struct v4l2_int_device *slave)
 	}
 
 	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, true, true);
+	vidioc_int_s_power(cam->sensor, 1);
 	vidioc_int_dev_init(slave);
 	ipu_csi_enable_mclk_if(CSI_MCLK_I2C, cam->csi, false, false);
 	cam_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -2623,53 +2733,55 @@ static void mxc_v4l2_master_detach(struct v4l2_int_device *slave)
 static __init int camera_init(void)
 {
 	u8 err = 0;
+	int i;
 
 	pr_debug("In MVC:camera_init\n");
 
-	/* Register the device driver structure. */
-	err = platform_driver_register(&mxc_v4l2_driver);
-	if (err != 0) {
-		pr_err("ERROR: v4l2 capture:camera_init: "
-			"platform_driver_register failed.\n");
-		return err;
-	}
+	for (i = 0; i < ARRAY_SIZE(mxc_v4l2_int_device); i++) {
+		/* Register the device driver structure. */
+		err = platform_driver_register(&mxc_v4l2_driver[i]);
+		if (err != 0) {
+			pr_err("ERROR: v4l2 capture:camera_init: "
+				"platform_driver_register failed.\n");
+			return err;
+		}
 
-	/* Create g_cam and initialize it. */
-	if ((g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL)) == NULL) {
-		pr_err("ERROR: v4l2 capture: failed to register camera\n");
-		platform_driver_unregister(&mxc_v4l2_driver);
-		return -1;
-	}
-	init_camera_struct(g_cam);
+		/* Create g_cam and initialize it. */
+		if ((g_cam [i] = kmalloc(sizeof(cam_data), GFP_KERNEL)) == NULL) {
+			pr_err("ERROR: v4l2 capture: failed to register camera\n");
+			platform_driver_unregister(&mxc_v4l2_driver[i]);
+			return -1;
+		}
+		init_camera_struct(g_cam [i], i);
 
-	/* Set up the v4l2 device and register it*/
-	mxc_v4l2_int_device.priv = g_cam;
-	/* This function contains a bug that won't let this be rmmod'd. */
-	v4l2_int_device_register(&mxc_v4l2_int_device);
+		/* Set up the v4l2 device and register it*/
+		mxc_v4l2_int_device[i].priv = g_cam [i];
+		/* This function contains a bug that won't let this be rmmod'd. */
+		v4l2_int_device_register(&mxc_v4l2_int_device[i]);
 
-	/* Register the I2C device */
-	err = platform_device_register(&mxc_v4l2_devices);
-	if (err != 0) {
-		pr_err("ERROR: v4l2 capture: camera_init: "
-		       "platform_device_register failed.\n");
-		platform_driver_unregister(&mxc_v4l2_driver);
-		kfree(g_cam);
-		g_cam = NULL;
-		return err;
-	}
+		/* Register the I2C device */
+		err = platform_device_register(&mxc_v4l2_devices[i]);
+		if (err != 0) {
+			pr_err("ERROR: v4l2 capture: camera_init: "
+				   "platform_device_register failed.\n");
+					platform_driver_unregister(&mxc_v4l2_driver[i]);
+					kfree(g_cam [i]);
+					g_cam [i] = NULL;
+			return err;
+		}
 
-	/* register v4l video device */
-	if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
-	    == -1) {
-		platform_device_unregister(&mxc_v4l2_devices);
-		platform_driver_unregister(&mxc_v4l2_driver);
-		kfree(g_cam);
-		g_cam = NULL;
-		pr_err("ERROR: v4l2 capture: video_register_device failed\n");
-		return -1;
+		/* register v4l video device */
+		if (video_register_device(g_cam[i]->video_dev, VFL_TYPE_GRABBER, video_nr)== -1) {
+			platform_device_unregister(&mxc_v4l2_devices[i]);
+			platform_driver_unregister(&mxc_v4l2_driver[i]);
+			kfree(g_cam[i]);
+			g_cam [i] = NULL;
+			pr_err("ERROR: v4l2 capture: video_register_device failed\n");
+			return -1;
+		}
+		pr_debug("   Video device registered: %s #%d\n",
+			g_cam[i]->video_dev->name, g_cam[i]->video_dev->minor);
 	}
-	pr_debug("   Video device registered: %s #%d\n",
-		 g_cam->video_dev->name, g_cam->video_dev->minor);
 
 	return err;
 }
@@ -2683,19 +2795,34 @@ static void __exit camera_exit(void)
 
 	pr_info("V4L2 unregistering video\n");
 
-	if (g_cam->open_count) {
+	if (g_cam[0]->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(&mxc_v4l2_int_device[0]);
+		video_unregister_device(g_cam[0]->video_dev);
+		platform_driver_unregister(&mxc_v4l2_driver[0]);
+		platform_device_unregister(&mxc_v4l2_devices[0]);
+
+		mxc_free_frame_buf(g_cam[0]);
+		kfree(g_cam[0]);
+		g_cam[0] = NULL;
+	}
+
+	if (g_cam[1]->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(&mxc_v4l2_int_device);
-		video_unregister_device(g_cam->video_dev);
-		platform_driver_unregister(&mxc_v4l2_driver);
-		platform_device_unregister(&mxc_v4l2_devices);
-
-		mxc_free_frame_buf(g_cam);
-		kfree(g_cam);
-		g_cam = NULL;
+		v4l2_int_device_unregister(&mxc_v4l2_int_device[1]);
+		video_unregister_device(g_cam[1]->video_dev);
+		platform_driver_unregister(&mxc_v4l2_driver[1]);
+		platform_device_unregister(&mxc_v4l2_devices[1]);
+
+		mxc_free_frame_buf(g_cam[1]);
+		kfree(g_cam[1]);
+		g_cam[1] = NULL;
 	}
 }
 
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
index 45a211a80a38..abaaaea48447 100644
--- a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
@@ -36,6 +36,7 @@
 #include <media/v4l2-dev.h>
 
 #define FRAME_NUM 3
+//#define FRAME_NUM 4
 
 /*!
  * v4l2 frame structure.
@@ -123,7 +124,7 @@ typedef struct _cam_data {
 	/* still image capture */
 	wait_queue_head_t still_queue;
 	int still_counter;
-	dma_addr_t still_buf;
+	dma_addr_t still_buf[2];
 	void *still_buf_vaddr;
 
 	/* overlay */
@@ -166,7 +167,7 @@ typedef struct _cam_data {
 	struct v4l2_rect crop_defrect;
 	struct v4l2_rect crop_current;
 
-	int (*enc_update_eba) (dma_addr_t eba, int *bufferNum);
+	int (*enc_update_eba) (int csi,dma_addr_t eba, int *bufferNum);
 	int (*enc_enable) (void *private);
 	int (*enc_disable) (void *private);
 	int (*enc_enable_csi) (void *private);
@@ -194,6 +195,7 @@ typedef struct _cam_data {
 	/* camera sensor interface */
 	struct camera_sensor *cam_sensor; 	/* old version */
 	struct v4l2_int_device *sensor;
+	struct timeval tv_wakeup; // TODO - for testing. Remove later
 } cam_data;
 
 #if defined(CONFIG_MXC_IPU_V1) || defined(CONFIG_VIDEO_MXC_EMMA_CAMERA) \
diff --git a/drivers/media/video/mxc/capture/ov3640.c b/drivers/media/video/mxc/capture/ov3640.c
index 29e61234e7f8..899945b7d071 100644
--- a/drivers/media/video/mxc/capture/ov3640.c
+++ b/drivers/media/video/mxc/capture/ov3640.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2005-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -26,7 +26,7 @@
 #define OV3640_VOLTAGE_ANALOG               2800000
 #define OV3640_VOLTAGE_DIGITAL_CORE         1500000
 #define OV3640_VOLTAGE_DIGITAL_IO           1800000
-
+#define OV3640_VOLTAGE_DIGITAL_GPO	    2800000
 
 /* Check these values! */
 #define MIN_FPS 15
@@ -40,8 +40,8 @@ enum ov3640_mode {
 	ov3640_mode_MIN = 0,
 	ov3640_mode_VGA_640_480 = 0,
 	ov3640_mode_QVGA_320_240 = 1,
-	ov3640_mode_QXGA_2048_1536 = 2,
-	ov3640_mode_XGA_1024_768 = 3,
+	ov3640_mode_XGA_1024_768 = 2,
+	ov3640_mode_QXGA_2048_1536 = 3,
 	ov3640_mode_NTSC_720_480 = 4,
 	ov3640_mode_PAL_720_576 = 5,
 	ov3640_mode_MAX = 5
@@ -93,6 +93,44 @@ struct sensor {
 } ov3640_data;
 
 static struct reg_value ov3640_setting_15fps_QXGA_2048_1536[] = {
+#if 0
+	/* The true 15fps QXGA setting. */
+	{0x3012, 0x80, 0, 0}, {0x304d, 0x41, 0, 0}, {0x3087, 0x16, 0, 0},
+	{0x30aa, 0x45, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
+	{0x30b2, 0x13, 0, 0}, {0x30d7, 0x10, 0, 0}, {0x309e, 0x00, 0, 0},
+	{0x3602, 0x26, 0, 0}, {0x3603, 0x4D, 0, 0}, {0x364c, 0x04, 0, 0},
+	{0x360c, 0x12, 0, 0}, {0x361e, 0x00, 0, 0}, {0x361f, 0x11, 0, 0},
+	{0x3633, 0x03, 0, 0}, {0x3629, 0x3c, 0, 0}, {0x300e, 0x33, 0, 0},
+	{0x300f, 0x21, 0, 0}, {0x3010, 0x20, 0, 0}, {0x3011, 0x00, 0, 0},
+	{0x304c, 0x81, 0, 0}, {0x3029, 0x47, 0, 0}, {0x3070, 0x00, 0, 0},
+	{0x3071, 0xEC, 0, 0}, {0x301C, 0x06, 0, 0}, {0x3072, 0x00, 0, 0},
+	{0x3073, 0xC5, 0, 0}, {0x301D, 0x07, 0, 0}, {0x3018, 0x38, 0, 0},
+	{0x3019, 0x30, 0, 0}, {0x301a, 0x61, 0, 0}, {0x307d, 0x00, 0, 0},
+	{0x3087, 0x02, 0, 0}, {0x3082, 0x20, 0, 0}, {0x303c, 0x08, 0, 0},
+	{0x303d, 0x18, 0, 0}, {0x303e, 0x06, 0, 0}, {0x303F, 0x0c, 0, 0},
+	{0x3030, 0x62, 0, 0}, {0x3031, 0x26, 0, 0}, {0x3032, 0xe6, 0, 0},
+	{0x3033, 0x6e, 0, 0}, {0x3034, 0xea, 0, 0}, {0x3035, 0xae, 0, 0},
+	{0x3036, 0xa6, 0, 0}, {0x3037, 0x6a, 0, 0}, {0x3015, 0x12, 0, 0},
+	{0x3014, 0x04, 0, 0}, {0x3013, 0xf7, 0, 0}, {0x3104, 0x02, 0, 0},
+	{0x3105, 0xfd, 0, 0}, {0x3106, 0x00, 0, 0}, {0x3107, 0xff, 0, 0},
+	{0x3308, 0xa5, 0, 0}, {0x3316, 0xff, 0, 0}, {0x3317, 0x00, 0, 0},
+	{0x3087, 0x02, 0, 0}, {0x3082, 0x20, 0, 0}, {0x3300, 0x13, 0, 0},
+	{0x3301, 0xd6, 0, 0}, {0x3302, 0xef, 0, 0}, {0x30b8, 0x20, 0, 0},
+	{0x30b9, 0x17, 0, 0}, {0x30ba, 0x04, 0, 0}, {0x30bb, 0x08, 0, 0},
+	{0x3100, 0x02, 0, 0}, {0x3304, 0x00, 0, 0}, {0x3400, 0x00, 0, 0},
+	{0x3404, 0x02, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
+	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
+	{0x3025, 0x18, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x0c, 0, 0},
+	{0x335f, 0x68, 0, 0}, {0x3360, 0x18, 0, 0}, {0x3361, 0x0c, 0, 0},
+	{0x3362, 0x68, 0, 0}, {0x3363, 0x08, 0, 0}, {0x3364, 0x04, 0, 0},
+	{0x3403, 0x42, 0, 0}, {0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0},
+	{0x308a, 0x06, 0, 0}, {0x308b, 0x00, 0, 0}, {0x3507, 0x06, 0, 0},
+	{0x350a, 0x4f, 0, 0}, {0x3600, 0xc4, 0, 0},
+#endif
+	/*
+	 * Only support 7.5fps for QXGA to workaround screen tearing issue
+	 * for 15fps when capturing still image.
+	 */
 	{0x3012, 0x80, 0, 0}, {0x304d, 0x45, 0, 0}, {0x30a7, 0x5e, 0, 0},
 	{0x3087, 0x16, 0, 0}, {0x309c, 0x1a, 0, 0}, {0x30a2, 0xe4, 0, 0},
 	{0x30aa, 0x42, 0, 0}, {0x30b0, 0xff, 0, 0}, {0x30b1, 0xff, 0, 0},
@@ -118,25 +156,9 @@ static struct reg_value ov3640_setting_15fps_QXGA_2048_1536[] = {
 	{0x30bb, 0x08, 0, 0}, {0x3507, 0x06, 0, 0}, {0x350a, 0x4f, 0, 0},
 	{0x3100, 0x02, 0, 0}, {0x3301, 0xde, 0, 0}, {0x3304, 0x00, 0, 0},
 	{0x3400, 0x00, 0, 0}, {0x3404, 0x02, 0, 0}, {0x3600, 0xc4, 0, 0},
-	{0x3302, 0xef, 0, 0}, {0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0},
-	{0x3022, 0x00, 0, 0}, {0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0},
-	{0x3025, 0x00, 0, 0}, {0x3026, 0x06, 0, 0}, {0x3027, 0x00, 0, 0},
-	{0x335f, 0x68, 0, 0}, {0x3360, 0x00, 0, 0}, {0x3361, 0x00, 0, 0},
-	{0x3362, 0x68, 0, 0}, {0x3363, 0x00, 0, 0}, {0x3364, 0x00, 0, 0},
-	{0x3403, 0x00, 0, 0}, {0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0},
-	{0x308a, 0x06, 0, 0}, {0x308b, 0x00, 0, 0}, {0x307c, 0x10, 0, 0},
-	{0x3090, 0xc0, 0, 0}, {0x304c, 0x84, 0, 0}, {0x308d, 0x04, 0, 0},
-	{0x3086, 0x03, 0, 0}, {0x3086, 0x00, 0, 0}, {0x3012, 0x00, 0, 0},
-	{0x3020, 0x01, 0, 0}, {0x3021, 0x1d, 0, 0}, {0x3022, 0x00, 0, 0},
-	{0x3023, 0x0a, 0, 0}, {0x3024, 0x08, 0, 0}, {0x3025, 0x18, 0, 0},
-	{0x3026, 0x06, 0, 0}, {0x3027, 0x0c, 0, 0}, {0x302a, 0x06, 0, 0},
-	{0x302b, 0x20, 0, 0}, {0x3075, 0x44, 0, 0}, {0x300d, 0x00, 0, 0},
-	{0x30d7, 0x00, 0, 0}, {0x3069, 0x40, 0, 0}, {0x303e, 0x01, 0, 0},
-	{0x303f, 0x80, 0, 0}, {0x3302, 0x20, 0, 0}, {0x335f, 0x68, 0, 0},
-	{0x3360, 0x18, 0, 0}, {0x3361, 0x0c, 0, 0}, {0x3362, 0x68, 0, 0},
-	{0x3363, 0x08, 0, 0}, {0x3364, 0x04, 0, 0}, {0x3403, 0x42, 0, 0},
 	{0x3088, 0x08, 0, 0}, {0x3089, 0x00, 0, 0}, {0x308a, 0x06, 0, 0},
-	{0x308b, 0x00, 0, 0},
+	{0x308b, 0x00, 0, 0}, {0x308d, 0x04, 0, 0}, {0x3086, 0x03, 0, 0},
+	{0x3086, 0x00, 0, 0}, {0x3011, 0x01, 0, 0},
 };
 
 static struct reg_value ov3640_setting_15fps_XGA_1024_768[] = {
@@ -674,6 +696,7 @@ static struct regulator *io_regulator;
 static struct regulator *core_regulator;
 static struct regulator *analog_regulator;
 static struct regulator *gpo_regulator;
+static struct mxc_camera_platform_data *camera_plat;
 
 static int ov3640_probe(struct i2c_client *adapter,
 				const struct i2c_device_id *device_id);
@@ -843,6 +866,10 @@ static int ioctl_s_power(struct v4l2_int_device *s, int on)
 		if (analog_regulator)
 			if (regulator_enable(analog_regulator) != 0)
 				return -EIO;
+		/* Make sure power on */
+		if (camera_plat->pwdn)
+			camera_plat->pwdn(0);
+
 	} else if (!on && sensor->on) {
 		if (analog_regulator)
 			regulator_disable(analog_regulator);
@@ -920,6 +947,10 @@ static int ioctl_s_parm(struct v4l2_int_device *s, struct v4l2_streamparm *a)
 	enum ov3640_frame_rate frame_rate;
 	int ret = 0;
 
+	/* Make sure power on */
+	if (camera_plat->pwdn)
+		camera_plat->pwdn(0);
+
 	switch (a->type) {
 	/* This is the only case currently handled. */
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
@@ -1286,17 +1317,25 @@ static int ov3640_probe(struct i2c_client *client,
 		gpo_regulator = regulator_get(&client->dev,
 					      plat_data->gpo_regulator);
 		if (!IS_ERR(gpo_regulator)) {
+			regulator_set_voltage(gpo_regulator,
+					      OV3640_VOLTAGE_DIGITAL_GPO,
+					      OV3640_VOLTAGE_DIGITAL_GPO);
 			if (regulator_enable(gpo_regulator) != 0) {
-				pr_err("%s:gpo3 enable error\n", __func__);
+				pr_err("%s:gpo enable error\n", __func__);
 				goto err4;
 			} else {
 				dev_dbg(&client->dev,
-					"%s:gpo3 enable ok\n", __func__);
+					"%s:gpo enable ok\n", __func__);
 			}
 		} else
 			gpo_regulator = NULL;
 	}
 
+	if (plat_data->pwdn)
+		plat_data->pwdn(0);
+
+	camera_plat = plat_data;
+
 	ov3640_int_device.priv = &ov3640_data;
 	retval = v4l2_int_device_register(&ov3640_int_device);
 
diff --git a/drivers/media/video/mxc/output/Makefile b/drivers/media/video/mxc/output/Makefile
index 1713fa3bf3ab..500442544902 100644
--- a/drivers/media/video/mxc/output/Makefile
+++ b/drivers/media/video/mxc/output/Makefile
@@ -6,6 +6,9 @@ endif
 ifeq ($(CONFIG_VIDEO_MXC_IPU_OUTPUT),y)
 	obj-$(CONFIG_VIDEO_MXC_OUTPUT)	+= mxc_v4l2_output.o
 endif
+ifeq ($(CONFIG_VIDEO_MXC_PXP_V4L2),y)
+	obj-$(CONFIG_VIDEO_MXC_PXP_V4L2)	+= mxc_pxp_v4l2.o
+endif
 ifeq ($(CONFIG_VIDEO_MXC_IPUV1_WVGA_OUTPUT),y)
 	obj-$(CONFIG_VIDEO_MXC_OUTPUT)	+= mx31_v4l2_wvga_output.o
 endif
diff --git a/drivers/media/video/mxc/output/mxc_v4l2_output.c b/drivers/media/video/mxc/output/mxc_v4l2_output.c
index 408f9b7871a8..b7bbfed73019 100644
--- a/drivers/media/video/mxc/output/mxc_v4l2_output.c
+++ b/drivers/media/video/mxc/output/mxc_v4l2_output.c
@@ -44,9 +44,7 @@ vout_data *g_vout;
 #define LOAD_3FIELDS(vout) ((INTERLACED_CONTENT(vout)) && \
 			    ((vout)->motion_sel != HIGH_MOTION))
 
-#define SDC_FG_FB_FORMAT        IPU_PIX_FMT_RGB565
-
-struct v4l2_output mxc_outputs[2] = {
+struct v4l2_output mxc_outputs[1] = {
 	{
 	 .index = MXC_V4L2_OUT_2_SDC,
 	 .name = "DISP3 Video Out",
@@ -54,23 +52,14 @@ struct v4l2_output mxc_outputs[2] = {
 						   but no other choice */
 	 .audioset = 0,
 	 .modulator = 0,
-	 .std = V4L2_STD_UNKNOWN},
-	{
-	 .index = MXC_V4L2_OUT_2_ADC,
-	 .name = "DISPx Video Out",
-	 .type = V4L2_OUTPUT_TYPE_ANALOG,	/* not really correct,
-						   but no other choice */
-	 .audioset = 0,
-	 .modulator = 0,
 	 .std = V4L2_STD_UNKNOWN}
 };
 
 static int video_nr = 16;
 static spinlock_t g_lock = SPIN_LOCK_UNLOCKED;
 static int last_index_n;
-static int last_index_c;
 static unsigned int ipu_ic_out_max_width_size;
-
+static unsigned int ipu_ic_out_max_height_size;
 /* debug counters */
 uint32_t g_irq_cnt;
 uint32_t g_buf_output_cnt;
@@ -256,10 +245,14 @@ static int select_display_buffer(vout_data *vout, int next_buf)
 {
 	int ret = 0;
 
+	vout->disp_buf_num = next_buf;
 	if (ipu_get_cur_buffer_idx(vout->display_ch, IPU_INPUT_BUFFER)
 			!= next_buf)
 		ret = ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER,
 				next_buf);
+	else
+		dev_dbg(&vout->video_dev->dev,
+			"display buffer not ready for select\n");
 	return ret;
 }
 
@@ -290,110 +283,138 @@ static void setup_next_buf_timer(vout_data *vout, int index)
 			"timer handler next schedule: %lu\n", timeout);
 }
 
-static int wait_for_disp_vsync(vout_data *vout)
+static int finish_previous_frame(vout_data *vout)
 {
 	struct fb_info *fbi =
 		registered_fb[vout->output_fb_num[vout->cur_disp_output]];
 	mm_segment_t old_fs;
 	int ret = 0;
 
-	/* wait for display frame finish */
-	if (fbi->fbops->fb_ioctl) {
-		old_fs = get_fs();
-		set_fs(KERNEL_DS);
-		ret = fbi->fbops->fb_ioctl(fbi, MXCFB_WAIT_FOR_VSYNC,
-			(unsigned int)NULL);
-		set_fs(old_fs);
+	/* make sure buf[vout->disp_buf_num] in showing */
+	while (ipu_check_buffer_busy(vout->display_ch,
+			IPU_INPUT_BUFFER, vout->disp_buf_num)) {
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			ret = fbi->fbops->fb_ioctl(fbi, MXCFB_WAIT_FOR_VSYNC,
+					(unsigned int)NULL);
+			set_fs(old_fs);
+
+			if (ret < 0) {
+				/* ic_bypass need clear display buffer ready for next update*/
+				ipu_clear_buffer_ready(vout->display_ch, IPU_INPUT_BUFFER,
+						vout->disp_buf_num);
+			}
+		}
+	}
+
+	return ret;
+}
+
+static int show_current_frame(vout_data *vout)
+{
+	struct fb_info *fbi =
+		registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	mm_segment_t old_fs;
+	int ret = 0;
+
+	/* make sure buf[vout->disp_buf_num] begin to show */
+	if (ipu_get_cur_buffer_idx(vout->display_ch, IPU_INPUT_BUFFER)
+		!= vout->disp_buf_num) {
+		/* wait for display frame finish */
+		if (fbi->fbops->fb_ioctl) {
+			old_fs = get_fs();
+			set_fs(KERNEL_DS);
+			ret = fbi->fbops->fb_ioctl(fbi, MXCFB_WAIT_FOR_VSYNC,
+					(unsigned int)NULL);
+			set_fs(old_fs);
+		}
 	}
+
 	return ret;
 }
 
-static void timer_work_func(struct work_struct *work)
+static void icbypass_work_func(struct work_struct *work)
 {
 	vout_data *vout =
-		container_of(work, vout_data, timer_work);
+		container_of(work, vout_data, icbypass_work);
 	int index, ret;
 	int last_buf;
 	unsigned long lock_flags = 0;
 
-	/* wait 2 first frame finish for ic bypass mode*/
-	if ((g_buf_output_cnt == 0) && vout->ic_bypass) {
-		wait_for_disp_vsync(vout);
-		wait_for_disp_vsync(vout);
-		spin_lock_irqsave(&g_lock, lock_flags);
-		last_buf = vout->ipu_buf[0];
-		vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
-		queue_buf(&vout->done_q, last_buf);
-		vout->ipu_buf[0] = -1;
-		last_buf = vout->ipu_buf[1];
-		vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
-		queue_buf(&vout->done_q, last_buf);
-		vout->ipu_buf[1] = -1;
-		g_buf_output_cnt = 2;
-		wake_up_interruptible(&vout->v4l_bufq);
-		if (vout->state == STATE_STREAM_PAUSED) {
-			index = peek_next_buf(&vout->ready_q);
-			if (index != -1) {
-				/* Setup timer for next buffer, when stream has been paused */
-				pr_debug("next index %d\n", index);
-				setup_next_buf_timer(vout, index);
-				vout->state = STATE_STREAM_ON;
-			}
-		}
-		spin_unlock_irqrestore(&g_lock, lock_flags);
-		return;
-	}
-
-	if (wait_for_disp_vsync(vout) < 0) {
-		/* ic_bypass need clear display buffer ready for next update*/
-		ipu_clear_buffer_ready(vout->display_ch, IPU_INPUT_BUFFER,
-			!vout->next_done_ipu_buf);
-	}
+	finish_previous_frame(vout);
 
 	spin_lock_irqsave(&g_lock, lock_flags);
 
-	if (vout->ic_bypass) {
-		last_buf = vout->ipu_buf[vout->next_done_ipu_buf];
-		if (last_buf != -1) {
-			g_buf_output_cnt++;
-			vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
-			queue_buf(&vout->done_q, last_buf);
-			wake_up_interruptible(&vout->v4l_bufq);
-			vout->ipu_buf[vout->next_done_ipu_buf] = -1;
-			vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
-		}
+	index = dequeue_buf(&vout->ready_q);
+	if (index == -1) {	/* no buffers ready, should never occur */
+		dev_err(&vout->video_dev->dev,
+				"mxc_v4l2out: timer - no queued buffers ready\n");
+		goto exit;
 	}
+	g_buf_dq_cnt++;
+	vout->frame_count++;
 
-	if (vout->ic_bypass)
-		ret = select_display_buffer(vout, vout->next_rdy_ipu_buf);
-	else if (LOAD_3FIELDS(vout))
-		ret = ipu_select_multi_vdi_buffer(vout->next_rdy_ipu_buf);
-	else
-		ret = ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
-				vout->next_rdy_ipu_buf);
+	vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
+	ret = ipu_update_channel_buffer(vout->display_ch, IPU_INPUT_BUFFER,
+			vout->next_rdy_ipu_buf,
+			vout->v4l2_bufs[index].m.offset);
+	ret += select_display_buffer(vout, vout->next_rdy_ipu_buf);
 	if (ret < 0) {
 		dev_err(&vout->video_dev->dev,
-				"unable to set IPU buffer ready\n");
+				"unable to update buffer %d address rc=%d\n",
+				vout->next_rdy_ipu_buf, ret);
+		goto exit;
 	}
+	spin_unlock_irqrestore(&g_lock, lock_flags);
+	show_current_frame(vout);
+	spin_lock_irqsave(&g_lock, lock_flags);
+	vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
 
-	/* Non IC split action */
-	if (!vout->pp_split)
-		vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
+	last_buf = vout->ipu_buf[vout->next_done_ipu_buf];
+	if (last_buf != -1) {
+		g_buf_output_cnt++;
+		vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+		queue_buf(&vout->done_q, last_buf);
+		wake_up_interruptible(&vout->v4l_bufq);
+		vout->ipu_buf[vout->next_done_ipu_buf] = -1;
+		vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+	}
 
-	/* Setup timer for next buffer */
-	index = peek_next_buf(&vout->ready_q);
-	if (index != -1)
-		setup_next_buf_timer(vout, index);
-	else
-		vout->state = STATE_STREAM_PAUSED;
+	if (g_buf_output_cnt > 0) {
+		/* Setup timer for next buffer */
+		index = peek_next_buf(&vout->ready_q);
+		if (index != -1)
+			setup_next_buf_timer(vout, index);
+		else
+			vout->state = STATE_STREAM_PAUSED;
 
+		if (vout->state == STATE_STREAM_STOPPING) {
+			if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
+				vout->state = STATE_STREAM_OFF;
+			}
+		}
+	}
+exit:
 	spin_unlock_irqrestore(&g_lock, lock_flags);
+}
 
-	if (vout->state == STATE_STREAM_STOPPING) {
-		if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
-			vout->state = STATE_STREAM_OFF;
-		}
+static int get_cur_fb_blank(vout_data *vout)
+{
+	struct fb_info *fbi =
+		registered_fb[vout->output_fb_num[vout->cur_disp_output]];
+	mm_segment_t old_fs;
+	int ret = 0;
+
+	if (fbi->fbops->fb_ioctl) {
+		old_fs = get_fs();
+		set_fs(KERNEL_DS);
+		ret = fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_BLANK,
+				(unsigned int)(&vout->fb_blank));
+		set_fs(old_fs);
 	}
+
+	return ret;
 }
 
 static void mxc_v4l2out_timer_handler(unsigned long arg)
@@ -401,13 +422,13 @@ static void mxc_v4l2out_timer_handler(unsigned long arg)
 	int index, ret;
 	unsigned long lock_flags = 0;
 	vout_data *vout = (vout_data *) arg;
-	unsigned int aid_field_offset = 0, current_field_offset = 0;
 
 	spin_lock_irqsave(&g_lock, lock_flags);
 
 	if ((vout->state == STATE_STREAM_STOPPING)
 	    || (vout->state == STATE_STREAM_OFF))
 		goto exit0;
+
 	/*
 	 * If timer occurs before IPU h/w is ready, then set the state to
 	 * paused and the timer will be set again when next buffer is queued
@@ -419,98 +440,92 @@ static void mxc_v4l2out_timer_handler(unsigned long arg)
 		goto exit0;
 	}
 
-	/* Dequeue buffer and pass to IPU */
-	if (INTERLACED_CONTENT(vout)) {
-		if (((LOAD_3FIELDS(vout)) && (vout->next_rdy_ipu_buf)) ||
-		    ((!LOAD_3FIELDS(vout)) && !(vout->next_rdy_ipu_buf))) {
-			aid_field_offset = vout->bytesperline;
-			current_field_offset = 0;
-			index = last_index_n;
-		} else {
-			aid_field_offset = 0;
-			current_field_offset = vout->bytesperline;
-			index = dequeue_buf(&vout->ready_q);
-			if (index == -1) {	/* no buffers ready, should never occur */
-				dev_err(&vout->video_dev->dev,
-					"mxc_v4l2out: timer - no queued buffers ready\n");
-				goto exit0;
-			}
-			g_buf_dq_cnt++;
-			vout->frame_count++;
-			last_index_n = index;
-		}
-	} else {
-		current_field_offset = 0;
-		index = dequeue_buf(&vout->ready_q);
-		if (index == -1) {	/* no buffers ready, should never occur */
+	/* VDI need both buffer done before update buffer? */
+	if (INTERLACED_CONTENT(vout) &&
+		(vout->ipu_buf[!vout->next_rdy_ipu_buf] != -1)) {
+		dev_dbg(&vout->video_dev->dev, "IPU buffer busy\n");
+		vout->state = STATE_STREAM_PAUSED;
+		goto exit0;
+	}
+
+	/* Handle ic bypass mode in work queue */
+	if (vout->ic_bypass) {
+		if (queue_work(vout->v4l_wq, &vout->icbypass_work) == 0) {
 			dev_err(&vout->video_dev->dev,
-				"mxc_v4l2out: timer - no queued buffers ready\n");
-			goto exit0;
+				"ic bypass work was in queue already!\n ");
+			vout->state = STATE_STREAM_PAUSED;
 		}
-		g_buf_dq_cnt++;
-		vout->frame_count++;
+		goto exit0;
+	} else if (!vout->fb_blank &&
+		(ipu_get_cur_buffer_idx(vout->display_ch, IPU_INPUT_BUFFER)
+		== vout->next_disp_ipu_buf)) {
+		dev_dbg(&vout->video_dev->dev, "IPU disp busy\n");
+		get_cur_fb_blank(vout);
+		index = peek_next_buf(&vout->ready_q);
+		setup_next_buf_timer(vout, index);
+		goto exit0;
 	}
+	vout->fb_blank = 0;
+
+	/* Dequeue buffer and pass to IPU */
+	index = dequeue_buf(&vout->ready_q);
+	if (index == -1) {	/* no buffers ready, should never occur */
+		dev_err(&vout->video_dev->dev,
+			"mxc_v4l2out: timer - no queued buffers ready\n");
+		goto exit0;
+	}
+	g_buf_dq_cnt++;
+	vout->frame_count++;
 
 	/* update next buffer */
-	if (vout->ic_bypass) {
+	if (LOAD_3FIELDS(vout)) {
+		int index_n = index;
+		int index_p = last_index_n;
+		vout->ipu_buf_p[vout->next_rdy_ipu_buf] = last_index_n;
 		vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
-		ret = ipu_update_channel_buffer(vout->display_ch, IPU_INPUT_BUFFER,
-				      vout->next_rdy_ipu_buf,
-				      vout->v4l2_bufs[index].m.offset);
+		vout->ipu_buf_n[vout->next_rdy_ipu_buf] = index;
+		ret = ipu_update_channel_buffer(vout->post_proc_ch,
+				IPU_INPUT_BUFFER,
+				vout->next_rdy_ipu_buf,
+				vout->v4l2_bufs[index].m.offset);
+		ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_P,
+				IPU_INPUT_BUFFER,
+				vout->next_rdy_ipu_buf,
+				vout->v4l2_bufs[index_p].m.offset + vout->bytesperline);
+		ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_N,
+				IPU_INPUT_BUFFER,
+				vout->next_rdy_ipu_buf,
+				vout->v4l2_bufs[index_n].m.offset) + vout->bytesperline;
+		last_index_n = index;
 	} else {
-		if (LOAD_3FIELDS(vout)) {
-			int index_n = index;
-			int index_p = last_index_c;
-			index = last_index_n;
-			vout->ipu_buf_p[vout->next_rdy_ipu_buf] = index_p;
-			vout->ipu_buf[vout->next_rdy_ipu_buf] = last_index_c = index;
-			vout->ipu_buf_n[vout->next_rdy_ipu_buf] = last_index_n = index_n;
-			last_index_n = vout->ipu_buf_n[vout->next_rdy_ipu_buf];
-			last_index_c = vout->ipu_buf[vout->next_rdy_ipu_buf];
+		vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
+		if (vout->pp_split) {
+			vout->ipu_buf[!vout->next_rdy_ipu_buf] = index;
+			/* always left stripe */
 			ret = ipu_update_channel_buffer(vout->post_proc_ch,
-							IPU_INPUT_BUFFER,
-							vout->next_rdy_ipu_buf,
-							vout->v4l2_bufs[index].m.offset+current_field_offset);
-			ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_P,
-							 IPU_INPUT_BUFFER,
-							 vout->next_rdy_ipu_buf,
-							 vout->v4l2_bufs[index_p].m.offset+aid_field_offset);
-			ret += ipu_update_channel_buffer(MEM_VDI_PRP_VF_MEM_N,
-							 IPU_INPUT_BUFFER,
-							 vout->next_rdy_ipu_buf,
-							 vout->v4l2_bufs[index_n].m.offset+aid_field_offset);
-		} else {
-			vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
-			if (vout->pp_split) {
-				vout->ipu_buf[!vout->next_rdy_ipu_buf] = index;
-				/* always left stripe */
-				ret = ipu_update_channel_buffer(vout->post_proc_ch,
-									IPU_INPUT_BUFFER,
-									0,/* vout->next_rdy_ipu_buf,*/
-									(vout->v4l2_bufs[index].m.offset) +
-									vout->pp_left_stripe.input_column +
-									current_field_offset);
-
-				/* the U/V offset has to be updated inside of IDMAC */
-				/* according to stripe offset */
-				ret += ipu_update_channel_offset(vout->post_proc_ch,
-									IPU_INPUT_BUFFER,
-									vout->v2f.fmt.pix.pixelformat,
-									vout->v2f.fmt.pix.width,
-									vout->v2f.fmt.pix.height,
-									vout->bytesperline,
-									vout->offset.u_offset,
-									vout->offset.v_offset,
-									0,
-									vout->pp_left_stripe.input_column + current_field_offset);
-
-			} else
-				ret = ipu_update_channel_buffer(vout->post_proc_ch,
-									IPU_INPUT_BUFFER,
-									vout->next_rdy_ipu_buf,
-									vout->v4l2_bufs[index].m.offset +
-									current_field_offset);
-		}
+					IPU_INPUT_BUFFER,
+					0,/* vout->next_rdy_ipu_buf,*/
+					(vout->v4l2_bufs[index].m.offset) +
+					vout->pp_left_stripe.input_column +
+					vout->pp_up_stripe.input_column * vout->bytesperline);
+
+			/* the U/V offset has to be updated inside of IDMAC */
+			/* according to stripe offset */
+			ret += ipu_update_channel_offset(vout->post_proc_ch,
+					IPU_INPUT_BUFFER,
+					vout->v2f.fmt.pix.pixelformat,
+					vout->v2f.fmt.pix.width,
+					vout->v2f.fmt.pix.height,
+					vout->bytesperline,
+					vout->offset.u_offset,
+					vout->offset.v_offset,
+					vout->pp_up_stripe.input_column,
+					vout->pp_left_stripe.input_column);
+		} else
+			ret = ipu_update_channel_buffer(vout->post_proc_ch,
+					IPU_INPUT_BUFFER,
+					vout->next_rdy_ipu_buf,
+					vout->v4l2_bufs[index].m.offset);
 	}
 
 	if (ret < 0) {
@@ -521,9 +536,35 @@ static void mxc_v4l2out_timer_handler(unsigned long arg)
 	}
 
 	/* set next buffer ready */
-	if (queue_work(vout->v4l_wq, &vout->timer_work) == 0) {
-		dev_err(&vout->video_dev->dev, "work was in queue already!\n ");
+	if (LOAD_3FIELDS(vout))
+		ret = ipu_select_multi_vdi_buffer(vout->next_rdy_ipu_buf);
+	else
+		ret = ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+				vout->next_rdy_ipu_buf);
+	if (ret < 0) {
+		dev_err(&vout->video_dev->dev,
+				"unable to set IPU buffer ready\n");
+		goto exit0;
+	}
+
+	/* Split mode use buf 0 only, no need swith buf */
+	if (!vout->pp_split)
+		vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
+
+	/* Always assume display in double buffers */
+	vout->next_disp_ipu_buf = !vout->next_disp_ipu_buf;
+
+	/* Setup timer for next buffer */
+	index = peek_next_buf(&vout->ready_q);
+	if (index != -1)
+		setup_next_buf_timer(vout, index);
+	else
 		vout->state = STATE_STREAM_PAUSED;
+
+	if (vout->state == STATE_STREAM_STOPPING) {
+		if ((vout->ipu_buf[0] == -1) && (vout->ipu_buf[1] == -1)) {
+			vout->state = STATE_STREAM_OFF;
+		}
 	}
 
 	spin_unlock_irqrestore(&g_lock, lock_flags);
@@ -540,12 +581,15 @@ static irqreturn_t mxc_v4l2out_work_irq_handler(int irq, void *dev_id)
 	int index;
 	unsigned long lock_flags = 0;
 	vout_data *vout = dev_id;
-	int pp_out_buf_num = 0;
+	int pp_out_buf_left_right = 0;
 	int disp_buf_num = 0;
 	int disp_buf_num_next = 1;
+	int local_buffer = 0;
 	int pp_out_buf_offset = 0;
+	int pp_out_buf_up_down = 0;
 	int release_buffer = 0;
-	u32 eba_offset;
+	u32 eba_offset = 0;
+	u32 vertical_offset = 0;
 	u16 x_pos;
 	u16 y_pos;
 	int ret = -1;
@@ -563,17 +607,37 @@ static irqreturn_t mxc_v4l2out_work_irq_handler(int irq, void *dev_id)
 	if (last_buf != -1) {
 		/* If IC split mode on, update output buffer number */
 		if (vout->pp_split) {
-			pp_out_buf_num = vout->pp_split_buf_num & 1;/* left/right stripe */
-			disp_buf_num = vout->pp_split_buf_num >> 1;
-			disp_buf_num_next = ((vout->pp_split_buf_num+2) & 3) >> 1;
-			if (!pp_out_buf_num) {/* next buffer is right stripe*/
-				eba_offset = vout->pp_right_stripe.input_column;/*always right stripe*/
+			pp_out_buf_up_down = vout->pp_split_buf_num & 1;/* left/right stripe */
+			pp_out_buf_left_right = (vout->pp_split_buf_num >> 1) & 1; /* up/down */
+			local_buffer = (vout->pp_split == 1) ? pp_out_buf_up_down :
+					pp_out_buf_left_right;
+			disp_buf_num = vout->pp_split_buf_num >> 2;
+			disp_buf_num_next =
+					((vout->pp_split_buf_num + (vout->pp_split << 0x1)) & 7) >> 2;
+			if ((!pp_out_buf_left_right) ||
+				((!pp_out_buf_up_down) && (vout->pp_split == 1))) {
+				if (vout->pp_split == 1) {
+						eba_offset = ((pp_out_buf_left_right + pp_out_buf_up_down) & 1) ?
+									vout->pp_right_stripe.input_column :
+									vout->pp_left_stripe.input_column;
+						vertical_offset = pp_out_buf_up_down ?
+									vout->pp_up_stripe.input_column :
+									vout->pp_down_stripe.input_column;
+
+				} else {
+						eba_offset = pp_out_buf_left_right ?
+									vout->pp_left_stripe.input_column :
+									vout->pp_right_stripe.input_column;
+						vertical_offset = pp_out_buf_left_right ?
+									vout->pp_up_stripe.input_column :
+									vout->pp_down_stripe.input_column;
+				}
+
 				ret = ipu_update_channel_buffer(vout->post_proc_ch,
 						IPU_INPUT_BUFFER,
-						1, /* right stripe */
+						(1 - local_buffer),
 						(vout->v4l2_bufs[vout->ipu_buf[disp_buf_num]].m.offset)
-						+ eba_offset);
-
+						+ eba_offset + vertical_offset * vout->bytesperline);
 				ret += ipu_update_channel_offset(vout->post_proc_ch,
 								IPU_INPUT_BUFFER,
 								vout->v2f.fmt.pix.pixelformat,
@@ -582,68 +646,78 @@ static irqreturn_t mxc_v4l2out_work_irq_handler(int irq, void *dev_id)
 								vout->bytesperline,
 								vout->offset.u_offset,
 								vout->offset.v_offset,
-								0,
-								vout->pp_right_stripe.input_column);
+								vertical_offset,
+								eba_offset);
 
 				/* select right stripe */
-				ret += ipu_select_buffer(vout->post_proc_ch,
-												IPU_INPUT_BUFFER, 1);
+				ret += ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
+							(1 - local_buffer));
 				if (ret < 0)
 					dev_err(&vout->video_dev->dev,
 					"unable to set IPU buffer ready\n");
+			}
 
-				vout->ipu_buf[vout->next_done_ipu_buf] = -1;
-				vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+			/* offset for next buffer's EBA */
+			eba_offset = 0;
+			if (vout->pp_split == 1) {
+				pp_out_buf_offset = ((vout->pp_split_buf_num >> 1) & 1) ?
+								vout->pp_left_stripe.output_column :
+								vout->pp_right_stripe.output_column;
 
-			} else /* right stripe is done, run display refresh */
-				select_display_buffer(vout, disp_buf_num);
+				eba_offset = ((vout->pp_split_buf_num & 1) ?
+								vout->pp_down_stripe.output_column :
+								vout->pp_up_stripe.output_column);
 
-			vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
+			} else {
+				pp_out_buf_offset = ((vout->pp_split_buf_num >> 1) & 1) ?
+									vout->pp_right_stripe.output_column :
+									vout->pp_left_stripe.output_column;
+				eba_offset = ((vout->pp_split_buf_num >> 1) & 1) ?
+									vout->pp_down_stripe.output_column :
+									vout->pp_up_stripe.output_column;
+			}
 
-			/* offset for next buffer's EBA */
-			pp_out_buf_offset = pp_out_buf_num ? vout->pp_right_stripe.output_column :
-				vout->pp_left_stripe.output_column;
-			eba_offset = 0;
 			if (vout->cur_disp_output == 5) {
 				x_pos = (vout->crop_current.left / 8) * 8;
 				y_pos = vout->crop_current.top;
-				eba_offset = (vout->xres * y_pos + x_pos) * vout->bpp / 8;
+				eba_offset += (vout->xres * y_pos + x_pos) * vout->bpp / 8;
 			}
 
+
 			/* next buffer update */
 			eba_offset = vout->display_bufs[disp_buf_num_next] +
-								pp_out_buf_offset + eba_offset;
+						pp_out_buf_offset + eba_offset;
 
 			ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
-												pp_out_buf_num, eba_offset);
+						local_buffer, eba_offset);
 
 			/* next buffer ready */
-			ret = ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, pp_out_buf_num);
+			ret = ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, local_buffer);
 
-			/* next stripe_buffer index 0..3 */
-			vout->pp_split_buf_num = (vout->pp_split_buf_num + 1) & 3;
+			/* next stripe_buffer index 0..7 */
+			vout->pp_split_buf_num = (vout->pp_split_buf_num + vout->pp_split) & 0x7;
 		} else {
-			/* show to display */
-			select_display_buffer(vout, vout->next_done_ipu_buf);
+			disp_buf_num = vout->next_done_ipu_buf;
 			ret += ipu_select_buffer(vout->display_input_ch, IPU_OUTPUT_BUFFER,
 					vout->next_done_ipu_buf);
 		}
 
 		/* release buffer. For split mode: if second stripe is done */
-		release_buffer = vout->pp_split ? pp_out_buf_num : 1;
+		release_buffer = vout->pp_split ? (!(vout->pp_split_buf_num & 0x3)) : 1;
 		if (release_buffer) {
-			if ((!INTERLACED_CONTENT(vout)) || (vout->next_done_ipu_buf)) {
-				g_buf_output_cnt++;
-				vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
-				queue_buf(&vout->done_q, last_buf);
-				wake_up_interruptible(&vout->v4l_bufq);
-			}
+			select_display_buffer(vout, disp_buf_num);
+			g_buf_output_cnt++;
+			vout->v4l2_bufs[last_buf].flags = V4L2_BUF_FLAG_DONE;
+			queue_buf(&vout->done_q, last_buf);
+			wake_up_interruptible(&vout->v4l_bufq);
 			vout->ipu_buf[vout->next_done_ipu_buf] = -1;
 			if (LOAD_3FIELDS(vout)) {
 				vout->ipu_buf_p[vout->next_done_ipu_buf] = -1;
 				vout->ipu_buf_n[vout->next_done_ipu_buf] = -1;
 			}
-			vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
+			/* split mode use buf 0 only, no need switch buf */
+			if (!vout->pp_split)
+				vout->next_done_ipu_buf = !vout->next_done_ipu_buf;
 		}
 	} /* end of last_buf != -1 */
 
@@ -702,7 +776,6 @@ static int init_VDI_channel(vout_data *vout, ipu_channel_params_t params)
 static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
 				   uint16_t in_width, uint16_t in_height,
 				   uint32_t stride,
-				   dma_addr_t phyaddr_0, dma_addr_t phyaddr_1,
 				   uint32_t u_offset, uint32_t v_offset)
 {
 	struct device *dev = &vout->video_dev->dev;
@@ -710,7 +783,7 @@ static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
 	if (ipu_init_channel_buffer(MEM_VDI_PRP_VF_MEM, IPU_INPUT_BUFFER,
 				    in_pixel_fmt, in_width, in_height, stride,
 				    IPU_ROTATE_NONE,
-				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset+vout->bytesperline,
+				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
 				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
 				    u_offset, v_offset) != 0) {
 		dev_err(dev, "Error initializing VDI current input buffer\n");
@@ -721,7 +794,7 @@ static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
 					    IPU_INPUT_BUFFER,
 					    in_pixel_fmt, in_width, in_height,
 					    stride, IPU_ROTATE_NONE,
-					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset,
+					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset+vout->bytesperline,
 					    vout->v4l2_bufs[vout->ipu_buf_p[0]].m.offset+vout->bytesperline,
 					    u_offset, v_offset) != 0) {
 			dev_err(dev, "Error initializing VDI previous input buffer\n");
@@ -731,7 +804,7 @@ static int init_VDI_in_channel_buffer(vout_data *vout, uint32_t in_pixel_fmt,
 					    IPU_INPUT_BUFFER,
 					    in_pixel_fmt, in_width, in_height,
 					    stride, IPU_ROTATE_NONE,
-					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset,
+					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset+vout->bytesperline,
 					    vout->v4l2_bufs[vout->ipu_buf_n[0]].m.offset+vout->bytesperline,
 					    u_offset, v_offset) != 0) {
 			dev_err(dev, "Error initializing VDI next input buffer\n");
@@ -759,10 +832,7 @@ static int init_VDI(ipu_channel_params_t params, vout_data *vout,
 	params.mem_prp_vf_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
 	params.mem_prp_vf_mem.out_width = out_width;
 	params.mem_prp_vf_mem.out_height = out_height;
-	if (vout->display_ch == ADC_SYS2)
-		params.mem_prp_vf_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
-	else
-		params.mem_prp_vf_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+	params.mem_prp_vf_mem.out_pixel_fmt = bpp_to_fmt(fbi);
 
 	if (init_VDI_channel(vout, params) != 0) {
 		dev_err(dev, "Error init_VDI_channel channel\n");
@@ -775,8 +845,6 @@ static int init_VDI(ipu_channel_params_t params, vout_data *vout,
 				       params.mem_prp_vf_mem.in_height,
 				       bytes_per_pixel(params.mem_prp_vf_mem.
 						       in_pixel_fmt),
-				       vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
-				       vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
 				       vout->offset.u_offset,
 				       vout->offset.v_offset) != 0) {
 		return -EINVAL;
@@ -805,7 +873,6 @@ static int init_VDI(ipu_channel_params_t params, vout_data *vout,
 			dev_err(dev, "Error initializing PRP output buffer\n");
 			return -EINVAL;
 		}
-
 		if (ipu_init_channel(MEM_ROT_VF_MEM, NULL) != 0) {
 			dev_err(dev, "Error initializing PP ROT channel\n");
 			return -EINVAL;
@@ -883,14 +950,14 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 	u32 eba_offset;
 	u16 x_pos;
 	u16 y_pos;
+	dma_addr_t phy_addr0;
+	dma_addr_t phy_addr1;
+
 	eba_offset = 0;
 	x_pos = 0;
 	y_pos = 0;
 
-	if (vout->display_ch == ADC_SYS2)
-		params->mem_pp_mem.out_pixel_fmt = SDC_FG_FB_FORMAT;
-	else
-		params->mem_pp_mem.out_pixel_fmt = bpp_to_fmt(fbi);
+	params->mem_pp_mem.out_pixel_fmt = bpp_to_fmt(fbi);
 
 	if (vout->cur_disp_output == 5) {
 		x_pos = (vout->crop_current.left / 8) * 8;
@@ -907,37 +974,66 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 	params->mem_pp_mem.in_pixel_fmt = vout->v2f.fmt.pix.pixelformat;
 	params->mem_pp_mem.out_width = out_width;
 	params->mem_pp_mem.out_height = out_height;
-	params->mem_pp_mem.out_resize_ratio = 0; /* 0 means unused */
-
+	params->mem_pp_mem.outh_resize_ratio = 0; /* 0 means unused */
+	params->mem_pp_mem.outv_resize_ratio = 0; /* 0 means unused */
 	/* split IC by two stripes, the by pass is impossible*/
 	if (vout->pp_split) {
-		ipu_calc_stripes_sizes(
-					params->mem_pp_mem.in_width, /* input frame width;>1 */
-					params->mem_pp_mem.out_width, /* output frame width; >1 */
-					ipu_ic_out_max_width_size,
-					(((unsigned long long)1) << 32), /* 32bit for fractional*/
-					1,	/* equal stripes */
-					params->mem_pp_mem.in_pixel_fmt,
-					params->mem_pp_mem.out_pixel_fmt,
-					&(vout->pp_left_stripe),
-					&(vout->pp_right_stripe));
-
-		vout->pp_left_stripe.input_column = vout->pp_left_stripe.input_column *
+		vout->pp_left_stripe.input_column = 0;
+		vout->pp_left_stripe.output_column = 0;
+		vout->pp_right_stripe.input_column = 0;
+		vout->pp_right_stripe.output_column = 0;
+		vout->pp_up_stripe.input_column = 0;
+		vout->pp_up_stripe.output_column = 0;
+		vout->pp_down_stripe.input_column = 0;
+		vout->pp_down_stripe.output_column = 0;
+		if (vout->pp_split != 3) {
+			ipu_calc_stripes_sizes(
+				params->mem_pp_mem.in_width, /* input frame width;>1 */
+				params->mem_pp_mem.out_width, /* output frame width; >1 */
+				ipu_ic_out_max_width_size,
+				(((unsigned long long)1) << 32), /* 32bit for fractional*/
+				1,	/* equal stripes */
+				params->mem_pp_mem.in_pixel_fmt,
+				params->mem_pp_mem.out_pixel_fmt,
+				&(vout->pp_left_stripe),
+				&(vout->pp_right_stripe));
+
+			vout->pp_left_stripe.input_column = vout->pp_left_stripe.input_column *
 								fmt_to_bpp(vout->v2f.fmt.pix.pixelformat) / 8;
-		vout->pp_left_stripe.output_column = vout->pp_left_stripe.output_column *
+			vout->pp_left_stripe.output_column = vout->pp_left_stripe.output_column *
 								fmt_to_bpp(params->mem_pp_mem.out_pixel_fmt) / 8;
-		vout->pp_right_stripe.input_column = vout->pp_right_stripe.input_column *
+			vout->pp_right_stripe.input_column = vout->pp_right_stripe.input_column *
 								fmt_to_bpp(vout->v2f.fmt.pix.pixelformat) / 8;
-		vout->pp_right_stripe.output_column = vout->pp_right_stripe.output_column *
+			vout->pp_right_stripe.output_column = vout->pp_right_stripe.output_column *
 								fmt_to_bpp(params->mem_pp_mem.out_pixel_fmt) / 8;
 
+
 		/* updare parameters */
 		params->mem_pp_mem.in_width = vout->pp_left_stripe.input_width;
 		params->mem_pp_mem.out_width = vout->pp_left_stripe.output_width;
 		out_width = vout->pp_left_stripe.output_width;
 		/* for using in ic_init*/
-		params->mem_pp_mem.out_resize_ratio = vout->pp_left_stripe.irr;
-
+		params->mem_pp_mem.outh_resize_ratio = vout->pp_left_stripe.irr;
+		}
+		if (vout->pp_split != 2) {
+			ipu_calc_stripes_sizes(
+				params->mem_pp_mem.in_height, /* input frame width;>1 */
+				params->mem_pp_mem.out_height, /* output frame width; >1 */
+				ipu_ic_out_max_height_size,
+				(((unsigned long long)1) << 32),/* 32bit for fractional */
+				1,	/* equal stripes */
+				params->mem_pp_mem.in_pixel_fmt,
+				params->mem_pp_mem.out_pixel_fmt,
+				&(vout->pp_up_stripe),
+				&(vout->pp_down_stripe));
+			vout->pp_down_stripe.output_column = vout->pp_down_stripe.output_column * out_stride;
+			vout->pp_up_stripe.output_column = vout->pp_up_stripe.output_column * out_stride;
+			params->mem_pp_mem.outv_resize_ratio = vout->pp_up_stripe.irr;
+			params->mem_pp_mem.in_height = vout->pp_up_stripe.input_width;/*height*/
+			out_height = vout->pp_up_stripe.output_width;/*height*/
+			if (vout->pp_split == 3)
+				vout->pp_split = 2;/*2 vertical stripe as two horizontal stripes */
+		}
 		vout->pp_split_buf_num = 0;
 	}
 
@@ -946,6 +1042,12 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 		return -EINVAL;
 	}
 
+	/* always enable double buffer */
+	phy_addr0 = vout->v4l2_bufs[vout->ipu_buf[0]].m.offset;
+	if (vout->ipu_buf[1] == -1)
+		phy_addr1 = phy_addr0;
+	else
+		phy_addr1 = vout->v4l2_bufs[vout->ipu_buf[1]].m.offset;
 	if (ipu_init_channel_buffer(vout->post_proc_ch,
 				    IPU_INPUT_BUFFER,
 				    params->mem_pp_mem.in_pixel_fmt,
@@ -955,8 +1057,8 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 				    bytes_per_pixel(params->mem_pp_mem.
 					    in_pixel_fmt),
 				    IPU_ROTATE_NONE,
-				    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
-				    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
+				    phy_addr0,
+				    phy_addr1,
 				    vout->offset.u_offset,
 				    vout->offset.v_offset) != 0) {
 		dev_err(dev, "Error initializing PP input buffer\n");
@@ -1048,20 +1150,27 @@ static int init_PP(ipu_channel_params_t *params, vout_data *vout,
 		ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
 									0,
 									vout->v4l2_bufs[vout->ipu_buf[0]].m.offset +
-									vout->pp_left_stripe.input_column);
+									vout->pp_left_stripe.input_column +
+									vout->pp_up_stripe.input_column * vout->bytesperline);
+
+
 		ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
 									1,
 									vout->v4l2_bufs[vout->ipu_buf[0]].m.offset +
-									vout->pp_right_stripe.input_column);
+									vout->pp_right_stripe.input_column +
+									vout->pp_up_stripe.input_column * vout->bytesperline);
+
 		ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
 									0,
 									vout->display_bufs[0] + eba_offset +
-									vout->pp_left_stripe.output_column);
+									vout->pp_left_stripe.output_column +
+									vout->pp_up_stripe.output_column);
 
 		ipu_update_channel_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER,
 									1,
 									vout->display_bufs[0] + eba_offset +
-									vout->pp_right_stripe.output_column);
+									vout->pp_right_stripe.output_column +
+									vout->pp_up_stripe.output_column);
 	}
 
 	return 0;
@@ -1084,8 +1193,8 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 	    registered_fb[vout->output_fb_num[vout->cur_disp_output]];
 	u16 out_width;
 	u16 out_height;
-	bool use_direct_adc = false;
 	mm_segment_t old_fs;
+	unsigned int ipu_ch = CHAN_NONE;
 	int rc = 0;
 
 	dev_dbg(dev, "mxc_v4l2out_streamon: field format=%d\n",
@@ -1107,241 +1216,132 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 		return -EINVAL;
 	}
 
+	/*
+	 * params init, check whether operation exceed the IC limitation:
+	 * whether split mode used ( ipu version >= ipuv3 only)
+	 */
 	g_irq_cnt = g_buf_output_cnt = g_buf_q_cnt = g_buf_dq_cnt = 0;
 	out_width = vout->crop_current.width;
 	out_height = vout->crop_current.height;
+	vout->disp_buf_num = 0;
 	vout->next_done_ipu_buf = 0;
-	vout->next_rdy_ipu_buf = 1;
+	vout->next_rdy_ipu_buf = vout->next_disp_ipu_buf = 1;
+	vout->fb_blank = 0;
 	vout->pp_split = 0;
-
-	if (!INTERLACED_CONTENT(vout)) {
-		vout->next_done_ipu_buf = vout->next_rdy_ipu_buf = 0;
-		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
+	ipu_ic_out_max_height_size = 1024;
 #ifdef CONFIG_MXC_IPU_V1
+	if (cpu_is_mx35())
 		ipu_ic_out_max_width_size = 800;
+	else
+		ipu_ic_out_max_width_size = 720;
 #else
-		ipu_ic_out_max_width_size = 1024;
+	ipu_ic_out_max_width_size = 1024;
 #endif
+	if ((out_width > ipu_ic_out_max_width_size) ||
+		(out_height > ipu_ic_out_max_height_size))
+		vout->pp_split = 4;
+	if (!INTERLACED_CONTENT(vout)) {
+		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
 		/* split IC by two stripes, the by pass is impossible*/
 		if ((out_width != vout->v2f.fmt.pix.width ||
 			out_height != vout->v2f.fmt.pix.height) &&
-			out_width > ipu_ic_out_max_width_size) {
-			vout->pp_split = 1;
+			vout->pp_split) {
+			vout->next_done_ipu_buf = vout->next_rdy_ipu_buf = 0;
 			vout->ipu_buf[1] = vout->ipu_buf[0];
 			vout->frame_count = 1;
+			if ((out_width > ipu_ic_out_max_width_size) &&
+				(out_height > ipu_ic_out_max_height_size))
+				vout->pp_split = 1; /*4 stripes*/
+			else if (!(out_height > ipu_ic_out_max_height_size))
+				vout->pp_split = 2; /*two horizontal stripes */
+			else
+				vout->pp_split = 3; /*2 vertical stripes*/
 		} else {
-			vout->ipu_buf[1] = dequeue_buf(&vout->ready_q);
-			vout->frame_count = 2;
+			vout->ipu_buf[1] = -1;
+			vout->frame_count = 1;
 		}
 	} else if (!LOAD_3FIELDS(vout)) {
 		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
 		vout->ipu_buf[1] = -1;
 		vout->frame_count = 1;
-		last_index_n = vout->ipu_buf[0];
 	} else {
 		vout->ipu_buf_p[0] = dequeue_buf(&vout->ready_q);
-		vout->ipu_buf[0] = vout->ipu_buf_p[0];
-		vout->ipu_buf_n[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf[0] = dequeue_buf(&vout->ready_q);
+		vout->ipu_buf_n[0] = vout->ipu_buf[0];
 		vout->ipu_buf_p[1] = -1;
 		vout->ipu_buf[1] = -1;
 		vout->ipu_buf_n[1] = -1;
-		last_index_c = vout->ipu_buf[0];
 		last_index_n = vout->ipu_buf_n[0];
 		vout->frame_count = 2;
 	}
 
-	/* Init Display Channel */
-#ifdef CONFIG_FB_MXC_ASYNC_PANEL
-	if (vout->cur_disp_output < DISP3) {
-		vout->work_irq = IPU_IRQ_PP_IN_EOF;
-		ipu_clear_irq(vout->work_irq);
-		ipu_request_irq(vout->work_irq,
-				mxc_v4l2out_work_irq_handler,
-				0, vout->video_dev->name, vout);
-		mxcfb_set_refresh_mode(fbi, MXCFB_REFRESH_OFF, 0);
-		fbi = NULL;
-		if (ipu_can_rotate_in_place(vout->rotate)) {
-			dev_dbg(dev, "Using PP direct to ADC channel\n");
-			use_direct_adc = true;
-			vout->display_ch = MEM_PP_ADC;
-			vout->post_proc_ch = MEM_PP_ADC;
-
-			memset(&params, 0, sizeof(params));
-			params.mem_pp_adc.in_width = vout->v2f.fmt.pix.width;
-			params.mem_pp_adc.in_height = vout->v2f.fmt.pix.height;
-			params.mem_pp_adc.in_pixel_fmt =
-			    vout->v2f.fmt.pix.pixelformat;
-			params.mem_pp_adc.out_width = out_width;
-			params.mem_pp_adc.out_height = out_height;
-			params.mem_pp_adc.out_pixel_fmt = SDC_FG_FB_FORMAT;
-#ifdef CONFIG_FB_MXC_EPSON_PANEL
-			params.mem_pp_adc.out_left =
-			    2 + vout->crop_current.left;
-#else
-			params.mem_pp_adc.out_left =
-			    12 + vout->crop_current.left;
-#endif
-			params.mem_pp_adc.out_top = vout->crop_current.top;
-			if (ipu_init_channel(vout->post_proc_ch, &params) != 0) {
-				dev_err(dev, "Error initializing PP chan\n");
-				return -EINVAL;
-			}
-			if (ipu_init_channel_buffer(vout->post_proc_ch,
-						    IPU_INPUT_BUFFER,
-						    params.mem_pp_adc.
-						    in_pixel_fmt,
-						    params.mem_pp_adc.in_width,
-						    params.mem_pp_adc.in_height,
-						    vout->v2f.fmt.pix.
-						    bytesperline /
-						    bytes_per_pixel(params.
-								    mem_pp_adc.
-								    in_pixel_fmt),
-						    vout->rotate,
-						    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset,
-						    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset,
-						    vout->offset.u_offset,
-						    vout->offset.v_offset) !=
-			    0) {
-				dev_err(dev, "Error initializing PP in buf\n");
-				return -EINVAL;
-			}
-
-			if (ipu_init_channel_buffer(vout->post_proc_ch,
-						    IPU_OUTPUT_BUFFER,
-						    params.mem_pp_adc.
-						    out_pixel_fmt, out_width,
-						    out_height, out_width,
-						    vout->rotate, 0, 0, 0,
-						    0) != 0) {
-				dev_err(dev,
-					"Error initializing PP output buffer\n");
-				return -EINVAL;
-			}
-
-		} else {
-			dev_dbg(dev, "Using ADC SYS2 channel\n");
-			vout->display_ch = ADC_SYS2;
-			vout->post_proc_ch = MEM_PP_MEM;
-
-			if (vout->display_bufs[0]) {
-				mxc_free_buffers(vout->display_bufs,
-						 vout->display_bufs_vaddr,
-						 2, vout->display_buf_size);
-			}
-
-			vout->display_buf_size = vout->crop_current.width *
-			    vout->crop_current.height *
-			    fmt_to_bpp(SDC_FG_FB_FORMAT) / 8;
-			mxc_allocate_buffers(vout->display_bufs,
-					     vout->display_bufs_vaddr,
-					     2, vout->display_buf_size);
-
-			memset(&params, 0, sizeof(params));
-			params.adc_sys2.disp = vout->cur_disp_output;
-			params.adc_sys2.ch_mode = WriteTemplateNonSeq;
-#ifdef CONFIG_FB_MXC_EPSON_PANEL
-			params.adc_sys2.out_left = 2 + vout->crop_current.left;
-#else
-			params.adc_sys2.out_left = 12 + vout->crop_current.left;
-#endif
-			params.adc_sys2.out_top = vout->crop_current.top;
-			if (ipu_init_channel(ADC_SYS2, &params) < 0)
-				return -EINVAL;
-
-			if (ipu_init_channel_buffer(vout->display_ch,
-						    IPU_INPUT_BUFFER,
-						    SDC_FG_FB_FORMAT,
-						    out_width, out_height,
-						    out_width, IPU_ROTATE_NONE,
-						    vout->display_bufs[0],
-						    vout->display_bufs[1], 0,
-						    0) != 0) {
-				dev_err(dev,
-					"Error initializing SDC FG buffer\n");
-				return -EINVAL;
-			}
-		}
-	} else
-#endif
-	{			/* Use SDC */
-		unsigned int ipu_ch = CHAN_NONE;
-
-		dev_dbg(dev, "Using SDC channel\n");
-
-		if (INTERLACED_CONTENT(vout))
-			vout->work_irq = IPU_IRQ_PRP_VF_OUT_EOF;
-		else
-			vout->work_irq = IPU_IRQ_PP_IN_EOF;
-
-		/*
-		 * Bypass IC if resizing and rotation are not needed
-		 * Meanwhile, apply IC bypass to SDC only
-		 */
-		fbvar = fbi->var;
-		vout->xres = fbvar.xres;
-		vout->yres = fbvar.yres;
-
-		if (vout->cur_disp_output == 3 || vout->cur_disp_output == 5) {
-			fbvar.bits_per_pixel = 16;
+	/*
+	 * Bypass IC if resizing and rotation are not needed
+	 * Meanwhile, apply IC bypass to SDC only
+	 */
+	fbvar = fbi->var;
+	vout->xres = fbvar.xres;
+	vout->yres = fbvar.yres;
+
+	if (vout->cur_disp_output == 3 || vout->cur_disp_output == 5) {
+		fbvar.bits_per_pixel = 16;
+		if (vout->cur_disp_output == 3) {
+			/* Only set YUV for the first display. The second display can
+			 * only work in RGB */
 			if (format_is_yuv(vout->v2f.fmt.pix.pixelformat))
 				fbvar.nonstd = IPU_PIX_FMT_UYVY;
 			else
 				fbvar.nonstd = 0;
-			if (vout->cur_disp_output == 3) {
-				fbvar.xres = out_width;
-				fbvar.yres = out_height;
-				vout->xres = fbvar.xres;
-				vout->yres = fbvar.yres;
-			}
 
-			fbvar.xres_virtual = fbvar.xres;
-			fbvar.yres_virtual = fbvar.yres * 2;
+			fbvar.xres = out_width;
+			fbvar.yres = out_height;
+			vout->xres = fbvar.xres;
+			vout->yres = fbvar.yres;
 		}
 
-		if (out_width == vout->v2f.fmt.pix.width &&
-			out_height == vout->v2f.fmt.pix.height &&
-			vout->xres == out_width &&
-			vout->yres == out_height &&
-			ipu_can_rotate_in_place(vout->rotate)) {
-			vout->ic_bypass = 1;
-		} else {
-			vout->ic_bypass = 0;
-		}
+		fbvar.xres_virtual = fbvar.xres;
+		fbvar.yres_virtual = fbvar.yres * 2;
+	}
+
+	if (out_width == vout->v2f.fmt.pix.width &&
+	    out_height == vout->v2f.fmt.pix.height &&
+	    vout->xres == out_width &&
+	    vout->yres == out_height &&
+	    ipu_can_rotate_in_place(vout->rotate) &&
+	    (vout->bytesperline ==
+	     bytes_per_pixel(vout->v2f.fmt.pix.pixelformat) * out_width) &&
+	    !INTERLACED_CONTENT(vout)) {
+		vout->ic_bypass = 1;
+	} else {
+		vout->ic_bypass = 0;
+	}
 
 #ifdef CONFIG_MXC_IPU_V1
-		/* IPUv1 needs IC to do CSC */
-		if (format_is_yuv(vout->v2f.fmt.pix.pixelformat) !=
-		    format_is_yuv(bpp_to_fmt(fbi)))
-			vout->ic_bypass = 0;
+	/* IPUv1 needs IC to do CSC */
+	if (format_is_yuv(vout->v2f.fmt.pix.pixelformat) !=
+	    format_is_yuv(bpp_to_fmt(fbi)))
+		vout->ic_bypass = 0;
 #endif
 
-		/* We are using IC to do input cropping */
-		if (vout->queue_buf_paddr[vout->ipu_buf[0]] !=
-		    vout->v4l2_bufs[vout->ipu_buf[0]].m.offset ||
-		    vout->queue_buf_paddr[vout->ipu_buf[1]] !=
-		    vout->v4l2_bufs[vout->ipu_buf[1]].m.offset)
-			vout->ic_bypass = 0;
-
-		if (fbi->fbops->fb_ioctl) {
-			old_fs = get_fs();
-			set_fs(KERNEL_DS);
-			fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_IPU_CHAN,
-					(unsigned long)&ipu_ch);
-			set_fs(old_fs);
-		}
+	if (fbi->fbops->fb_ioctl) {
+		old_fs = get_fs();
+		set_fs(KERNEL_DS);
+		fbi->fbops->fb_ioctl(fbi, MXCFB_GET_FB_IPU_CHAN,
+				(unsigned long)&ipu_ch);
+		set_fs(old_fs);
+	}
 
-		if (ipu_ch == CHAN_NONE) {
-			dev_err(dev, "Can not get display ipu channel\n");
-			return -EINVAL;
-		}
+	if (ipu_ch == CHAN_NONE) {
+		dev_err(dev, "Can not get display ipu channel\n");
+		return -EINVAL;
+	}
 
-		vout->display_ch = ipu_ch;
+	vout->display_ch = ipu_ch;
 
-		if (vout->ic_bypass) {
-			pr_debug("Bypassing IC\n");
-			vout->work_irq = -1;
-			switch (vout->v2f.fmt.pix.pixelformat) {
+	if (vout->ic_bypass) {
+		pr_debug("Bypassing IC\n");
+		vout->pp_split = 0;
+		switch (vout->v2f.fmt.pix.pixelformat) {
 			case V4L2_PIX_FMT_YUV420:
 			case V4L2_PIX_FMT_YVU420:
 			case V4L2_PIX_FMT_NV12:
@@ -1353,87 +1353,80 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 			default:
 				fbvar.bits_per_pixel = 8*
 					bytes_per_pixel(vout->v2f.fmt.pix.pixelformat);
-			}
-			fbvar.nonstd = vout->v2f.fmt.pix.pixelformat;
 		}
+		fbvar.nonstd = vout->v2f.fmt.pix.pixelformat;
+	}
 
-		fbvar.activate |= FB_ACTIVATE_FORCE;
-		fb_set_var(fbi, &fbvar);
+	/* Init display channel through fb API */
+	fbvar.activate |= FB_ACTIVATE_FORCE;
+	fb_set_var(fbi, &fbvar);
 
-		if (fbi->fbops->fb_ioctl && vout->display_ch == MEM_FG_SYNC) {
-			fb_pos.x = vout->crop_current.left;
-			fb_pos.y = vout->crop_current.top;
-			old_fs = get_fs();
-			set_fs(KERNEL_DS);
-			fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
-					     (unsigned long)&fb_pos);
-			set_fs(old_fs);
-		}
+	if (fbi->fbops->fb_ioctl && vout->display_ch == MEM_FG_SYNC) {
+		fb_pos.x = vout->crop_current.left;
+		fb_pos.y = vout->crop_current.top;
+		old_fs = get_fs();
+		set_fs(KERNEL_DS);
+		fbi->fbops->fb_ioctl(fbi, MXCFB_SET_OVERLAY_POS,
+				(unsigned long)&fb_pos);
+		set_fs(old_fs);
+	}
+
+	vout->display_bufs[1] = fbi->fix.smem_start;
+	vout->display_bufs[0] = fbi->fix.smem_start +
+		(fbi->fix.line_length * vout->yres);
+	vout->display_buf_size = vout->xres *
+		vout->yres * fbi->var.bits_per_pixel / 8;
+
+	/* fill black color for init fb, we assume fb has double buffer*/
+	if (format_is_yuv(vout->v2f.fmt.pix.pixelformat)) {
+		int i;
 
-		vout->display_bufs[1] = fbi->fix.smem_start;
-		vout->display_bufs[0] = fbi->fix.smem_start +
-			(fbi->fix.line_length * vout->yres);
-		vout->display_buf_size = vout->xres *
-			vout->yres * fbi->var.bits_per_pixel / 8;
-
-		/* fill black color for init fb, we assume fb has double buffer*/
-		if (format_is_yuv(vout->v2f.fmt.pix.pixelformat)) {
-			int i;
-
-			if ((vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY) ||
-				(vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) ||
-				(!vout->ic_bypass)) {
-				short * tmp = (short *) fbi->screen_base;
-				short color;
-				if (vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV)
-					color = 0x8000;
-				else
-					color = 0x80;
-				for (i = 0; i < (fbi->fix.line_length * fbi->var.yres_virtual)/2;
+		if ((vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY) ||
+			(vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) ||
+			(!vout->ic_bypass)) {
+			short * tmp = (short *) fbi->screen_base;
+			short color;
+			if (vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV)
+				color = 0x8000;
+			else
+				color = 0x80;
+			for (i = 0; i < (fbi->fix.line_length * fbi->var.yres_virtual)/2;
 					i++, tmp++)
-					*tmp = color;
-			} else if ((vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) ||
+				*tmp = color;
+		} else if ((vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) ||
 				(vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YVU420) ||
 				(vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_NV12)) {
-				char * base = (char *)fbi->screen_base;
-				int j, screen_size = fbi->var.xres * fbi->var.yres;
-
-				for (j = 0; j < 2; j++) {
-					memset(base, 0, screen_size);
-					base += screen_size;
-					for (i = 0; i < screen_size/2; i++, base++)
-						*base = 0x80;
-				}
-			} else if (vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P) {
-				char * base = (char *)fbi->screen_base;
-				int j, screen_size = fbi->var.xres * fbi->var.yres;
-
-				for (j = 0; j < 2; j++) {
-					memset(base, 0, screen_size);
-					base += screen_size;
-					for (i = 0; i < screen_size; i++, base++)
-						*base = 0x80;
-				}
+			char * base = (char *)fbi->screen_base;
+			int j, screen_size = fbi->var.xres * fbi->var.yres;
+
+			for (j = 0; j < 2; j++) {
+				memset(base, 0, screen_size);
+				base += screen_size;
+				for (i = 0; i < screen_size/2; i++, base++)
+					*base = 0x80;
+			}
+		} else if (vout->v2f.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV422P) {
+			char * base = (char *)fbi->screen_base;
+			int j, screen_size = fbi->var.xres * fbi->var.yres;
+
+			for (j = 0; j < 2; j++) {
+				memset(base, 0, screen_size);
+				base += screen_size;
+				for (i = 0; i < screen_size; i++, base++)
+					*base = 0x80;
 			}
-		} else
-			memset(fbi->screen_base, 0x0,
-					fbi->fix.line_length * fbi->var.yres_virtual);
-
-		if (INTERLACED_CONTENT(vout))
-			vout->post_proc_ch = MEM_VDI_PRP_VF_MEM;
-		else
-			vout->post_proc_ch = MEM_PP_MEM;
-
-		if (!vout->ic_bypass) {
-			ipu_clear_irq(vout->work_irq);
-			ipu_request_irq(vout->work_irq,
-					mxc_v4l2out_work_irq_handler,
-					0, vout->video_dev->name, vout);
 		}
-	}
+	} else
+		memset(fbi->screen_base, 0x0,
+				fbi->fix.line_length * fbi->var.yres_virtual);
+
+	if (INTERLACED_CONTENT(vout))
+		vout->post_proc_ch = MEM_VDI_PRP_VF_MEM;
+	else if (!vout->ic_bypass)
+		vout->post_proc_ch = MEM_PP_MEM;
 
-	/* Init PP */
-	if (use_direct_adc == false && !vout->ic_bypass) {
+	/* Init IC channel */
+	if (!vout->ic_bypass) {
 		if (vout->rotate >= IPU_ROTATE_90_RIGHT) {
 			out_width = vout->crop_current.height;
 			out_height = vout->crop_current.width;
@@ -1441,7 +1434,11 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 		vout->display_input_ch = vout->post_proc_ch;
 		memset(&params, 0, sizeof(params));
 		if (INTERLACED_CONTENT(vout)) {
-			rc = init_VDI(params, vout, dev, fbi, out_width, out_height);
+			if (vout->pp_split) {
+				dev_err(&vout->video_dev->dev, "VDI split has not supported yet.\n");
+				return -1;
+			} else
+				rc = init_VDI(params, vout, dev, fbi, out_width, out_height);
 		} else {
 			rc = init_PP(&params, vout, dev, fbi, out_width, out_height);
 		}
@@ -1449,52 +1446,79 @@ static int mxc_v4l2out_streamon(vout_data * vout)
 			return rc;
 	}
 
+	if (!vout->ic_bypass) {
+		switch (vout->display_input_ch) {
+		case MEM_PP_MEM:
+			vout->work_irq = IPU_IRQ_PP_OUT_EOF;
+			break;
+		case MEM_VDI_PRP_VF_MEM:
+			vout->work_irq = IPU_IRQ_PRP_VF_OUT_EOF;
+			break;
+		case MEM_ROT_VF_MEM:
+			vout->work_irq = IPU_IRQ_PRP_VF_ROT_OUT_EOF;
+			break;
+		case MEM_ROT_PP_MEM:
+			vout->work_irq = IPU_IRQ_PP_ROT_OUT_EOF;
+			break;
+		default:
+			dev_err(&vout->video_dev->dev,
+				"not support channel, should not be here\n");
+		}
+	} else
+		vout->work_irq = -1;
+
+	if (!vout->ic_bypass && (vout->work_irq > 0)) {
+		ipu_clear_irq(vout->work_irq);
+		ipu_request_irq(vout->work_irq,
+				mxc_v4l2out_work_irq_handler,
+				0, vout->video_dev->name, vout);
+	}
+
 	vout->state = STATE_STREAM_PAUSED;
 
-	if (use_direct_adc == false) {
-		if (fbi) {
-			acquire_console_sem();
-			fb_blank(fbi, FB_BLANK_UNBLANK);
-			release_console_sem();
-		} else {
-			ipu_enable_channel(vout->display_ch);
-		}
-		if (!vout->ic_bypass) {
+	/* Enable display and IC channels */
+	if (fbi) {
+		acquire_console_sem();
+		fb_blank(fbi, FB_BLANK_UNBLANK);
+		release_console_sem();
+	} else {
+		ipu_enable_channel(vout->display_ch);
+	}
+	if (!vout->ic_bypass) {
 #ifndef CONFIG_MXC_IPU_V1
-			ipu_enable_channel(vout->post_proc_ch);
+		ipu_enable_channel(vout->post_proc_ch);
 #endif
-			if (LOAD_3FIELDS(vout)) {
-				ipu_enable_channel(MEM_VDI_PRP_VF_MEM_P);
-				ipu_enable_channel(MEM_VDI_PRP_VF_MEM_N);
-				ipu_select_multi_vdi_buffer(0);
-			} else if (INTERLACED_CONTENT(vout)) {
-				ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
-			} else {
-				ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
-				if (!vout->pp_split)
-					ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1);
-			}
-			ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0);
-			ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 1);
+		if (LOAD_3FIELDS(vout)) {
+			ipu_enable_channel(MEM_VDI_PRP_VF_MEM_P);
+			ipu_enable_channel(MEM_VDI_PRP_VF_MEM_N);
+			ipu_select_multi_vdi_buffer(0);
+		} else
+			ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
+		ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 0);
+		ipu_select_buffer(vout->post_proc_ch, IPU_OUTPUT_BUFFER, 1);
 #ifdef CONFIG_MXC_IPU_V1
-			ipu_enable_channel(vout->post_proc_ch);
+		ipu_enable_channel(vout->post_proc_ch);
 #endif
-		} else {
-			ipu_update_channel_buffer(vout->display_ch,
+	} else {
+		ipu_update_channel_buffer(vout->display_ch,
 				IPU_INPUT_BUFFER,
 				0, vout->v4l2_bufs[vout->ipu_buf[0]].m.offset);
-			ipu_update_channel_buffer(vout->display_ch,
-				IPU_INPUT_BUFFER,
-				1, vout->v4l2_bufs[vout->ipu_buf[1]].m.offset);
-			ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 0);
-			ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 1);
-			queue_work(vout->v4l_wq, &vout->timer_work);
-		}
-	} else {
-		ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 0);
-		ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER, 1);
-		ipu_enable_channel(vout->post_proc_ch);
+		if (vout->offset.u_offset || vout->offset.v_offset)
+			/* only update u/v offset */
+			ipu_update_channel_offset(vout->display_ch,
+					IPU_INPUT_BUFFER,
+					vout->v2f.fmt.pix.pixelformat,
+					vout->v2f.fmt.pix.width,
+					vout->v2f.fmt.pix.height,
+					vout->bytesperline,
+					vout->offset.u_offset,
+					vout->offset.v_offset,
+					0,
+					0);
+		ipu_select_buffer(vout->display_ch, IPU_INPUT_BUFFER, 0);
+		queue_work(vout->v4l_wq, &vout->icbypass_work);
 	}
+
 	vout->start_jiffies = jiffies;
 
 	msleep(1);
@@ -1529,7 +1553,8 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 	if (!vout->ic_bypass)
 		ipu_free_irq(vout->work_irq, vout);
 
-	cancel_work_sync(&vout->timer_work);
+	if (vout->ic_bypass)
+		cancel_work_sync(&vout->icbypass_work);
 
 	spin_lock_irqsave(&g_lock, lockflag);
 
@@ -1556,9 +1581,21 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 		}
 	}
 
-	if (vout->post_proc_ch == MEM_PP_MEM ||
+	if (vout->ic_bypass) {
+		fbi->var.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbi->var);
+
+		if (vout->display_ch == MEM_FG_SYNC) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_POWERDOWN);
+			release_console_sem();
+		}
+
+		vout->display_bufs[0] = 0;
+		vout->display_bufs[1] = 0;
+	} else if (vout->post_proc_ch == MEM_PP_MEM ||
 	    vout->post_proc_ch == MEM_PRP_VF_MEM) {
-		/* SDC or ADC with Rotation */
+		/* SDC with Rotation */
 		if (!ipu_can_rotate_in_place(vout->rotate)) {
 			ipu_unlink_channels(MEM_PP_MEM, MEM_ROT_PP_MEM);
 			ipu_disable_channel(MEM_ROT_PP_MEM, true);
@@ -1571,28 +1608,23 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 		}
 		ipu_disable_channel(MEM_PP_MEM, true);
 
-		if (vout->display_ch == ADC_SYS2 ||
-			vout->display_ch == MEM_FG_SYNC) {
-			ipu_disable_channel(vout->display_ch, true);
-			ipu_uninit_channel(vout->display_ch);
-		} else {
-			fbi->var.activate |= FB_ACTIVATE_FORCE;
-			fb_set_var(fbi, &fbi->var);
+		fbi->var.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbi->var);
 
-			if (vout->display_ch == MEM_FG_SYNC) {
-				acquire_console_sem();
-				fb_blank(fbi, FB_BLANK_POWERDOWN);
-				release_console_sem();
-			}
-
-			vout->display_bufs[0] = 0;
-			vout->display_bufs[1] = 0;
+		if (vout->display_ch == MEM_FG_SYNC) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_POWERDOWN);
+			release_console_sem();
 		}
 
+		vout->display_bufs[0] = 0;
+		vout->display_bufs[1] = 0;
+
 		ipu_uninit_channel(MEM_PP_MEM);
 		if (!ipu_can_rotate_in_place(vout->rotate))
 			ipu_uninit_channel(MEM_ROT_PP_MEM);
-	} else if (INTERLACED_CONTENT(vout) && (vout->post_proc_ch == MEM_VDI_PRP_VF_MEM)) {
+	} else if (INTERLACED_CONTENT(vout) &&
+		(vout->post_proc_ch == MEM_VDI_PRP_VF_MEM)) {
 		if (!ipu_can_rotate_in_place(vout->rotate)) {
 			ipu_unlink_channels(MEM_VDI_PRP_VF_MEM,
 					    MEM_ROT_VF_MEM);
@@ -1606,32 +1638,32 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 		}
 
 		ipu_disable_channel(MEM_VDI_PRP_VF_MEM, true);
+		if (LOAD_3FIELDS(vout)) {
+			ipu_disable_channel(MEM_VDI_PRP_VF_MEM_P, true);
+			ipu_disable_channel(MEM_VDI_PRP_VF_MEM_N, true);
+		}
 
-		if (vout->display_ch == ADC_SYS2 ||
-			vout->display_ch == MEM_FG_SYNC) {
-			ipu_disable_channel(vout->display_ch, true);
-			ipu_uninit_channel(vout->display_ch);
-		} else {
-			fbi->var.activate |= FB_ACTIVATE_FORCE;
-			fb_set_var(fbi, &fbi->var);
+		fbi->var.activate |= FB_ACTIVATE_FORCE;
+		fb_set_var(fbi, &fbi->var);
 
-			if (vout->display_ch == MEM_FG_SYNC) {
-				acquire_console_sem();
-				fb_blank(fbi, FB_BLANK_POWERDOWN);
-				release_console_sem();
-			}
-
-			vout->display_bufs[0] = 0;
-			vout->display_bufs[1] = 0;
+		if (vout->display_ch == MEM_FG_SYNC) {
+			acquire_console_sem();
+			fb_blank(fbi, FB_BLANK_POWERDOWN);
+			release_console_sem();
 		}
 
+		vout->display_bufs[0] = 0;
+		vout->display_bufs[1] = 0;
+
 		ipu_uninit_channel(MEM_VDI_PRP_VF_MEM);
+		if (LOAD_3FIELDS(vout)) {
+			ipu_uninit_channel(MEM_VDI_PRP_VF_MEM_P);
+			ipu_uninit_channel(MEM_VDI_PRP_VF_MEM_N);
+		}
 		if (!ipu_can_rotate_in_place(vout->rotate))
 			ipu_uninit_channel(MEM_ROT_VF_MEM);
-	} else {		/* ADC Direct */
-		ipu_disable_channel(MEM_PP_ADC, true);
-		ipu_uninit_channel(MEM_PP_ADC);
 	}
+
 	vout->ready_q.head = vout->ready_q.tail = 0;
 	vout->done_q.head = vout->done_q.tail = 0;
 	for (i = 0; i < vout->buffer_cnt; i++) {
@@ -1640,23 +1672,9 @@ static int mxc_v4l2out_streamoff(vout_data * vout)
 		vout->v4l2_bufs[i].timestamp.tv_usec = 0;
 	}
 
+	vout->post_proc_ch = CHAN_NONE;
 	vout->state = STATE_STREAM_OFF;
 
-#ifdef CONFIG_FB_MXC_ASYNC_PANEL
-	if (vout->cur_disp_output < DISP3) {
-		if (vout->display_bufs[0] != 0) {
-			mxc_free_buffers(vout->display_bufs,
-					 vout->display_bufs_vaddr, 2,
-					 vout->display_buf_size);
-		}
-
-		mxcfb_set_refresh_mode(registered_fb
-				       [vout->
-					output_fb_num[vout->cur_disp_output]],
-				       MXCFB_REFRESH_PARTIAL, 0);
-	}
-#endif
-
 	return retval;
 }
 
@@ -1751,7 +1769,6 @@ static int mxc_v4l2out_s_fmt(vout_data * vout, struct v4l2_format *f)
 		dev_err(&vout->video_dev->dev,
 			"De-interlacing not supported in this device!\n");
 		vout->field_fmt = V4L2_FIELD_NONE;
-		break;
 	case V4L2_FIELD_INTERLACED_BT:
 		dev_err(&vout->video_dev->dev,
 			"V4L2_FIELD_INTERLACED_BT field format not supported yet!\n");
@@ -1898,7 +1915,7 @@ static int mxc_v4l2out_open(struct file *file)
 			goto oops;
 		}
 
-		INIT_WORK(&vout->timer_work, timer_work_func);
+		INIT_WORK(&vout->icbypass_work, icbypass_work_func);
 	}
 
 	file->private_data = dev;
@@ -2284,12 +2301,8 @@ mxc_v4l2out_do_ioctl(struct file *file,
 				break;
 			}
 
-			if (output->index < 3) {
-				*output = mxc_outputs[MXC_V4L2_OUT_2_ADC];
-				output->name[4] = '0' + output->index;
-			} else {
+			if (output->index >= 3)
 				*output = mxc_outputs[MXC_V4L2_OUT_2_SDC];
-			}
 			break;
 		}
 	case VIDIOC_G_OUTPUT:
diff --git a/drivers/media/video/mxc/output/mxc_v4l2_output.h b/drivers/media/video/mxc/output/mxc_v4l2_output.h
index 0dd8eb0076a9..c26194f5ff90 100644
--- a/drivers/media/video/mxc/output/mxc_v4l2_output.h
+++ b/drivers/media/video/mxc/output/mxc_v4l2_output.h
@@ -39,7 +39,6 @@
 
 #define MXC_V4L2_OUT_NUM_OUTPUTS        6
 #define MXC_V4L2_OUT_2_SDC              0
-#define MXC_V4L2_OUT_2_ADC              1
 
 
 typedef struct {
@@ -80,7 +79,9 @@ typedef struct _vout_data {
 
 	struct timer_list output_timer;
 	struct workqueue_struct *v4l_wq;
-	struct work_struct timer_work;
+	struct work_struct icbypass_work;
+	int disp_buf_num;
+	int fb_blank;
 	unsigned long start_jiffies;
 	u32 frame_count;
 
@@ -89,6 +90,7 @@ typedef struct _vout_data {
 
 	s8 next_rdy_ipu_buf;
 	s8 next_done_ipu_buf;
+	s8 next_disp_ipu_buf;
 	s8 ipu_buf[2];
 	s8 ipu_buf_p[2];
 	s8 ipu_buf_n[2];
@@ -141,8 +143,10 @@ typedef struct _vout_data {
 	int pp_split; /* 0,1 */
 	struct stripe_param pp_left_stripe;
 	struct stripe_param pp_right_stripe; /* struct for split parameters */
-	/* IC ouput buffer number. Counting from 0 to 3 */
-	int pp_split_buf_num; /*  0..3 */
+	struct stripe_param pp_up_stripe;
+	struct stripe_param pp_down_stripe;
+	/* IC ouput buffer number. Counting from 0 to 7 */
+	int pp_split_buf_num; /*  0..7 */
 	u16 bpp ; /* bit per pixel */
 	u16 xres; /* width of physical frame (BGs) */
 	u16 yres; /* heigth of physical frame (BGs)*/
diff --git a/drivers/media/video/mxs_pxp.c b/drivers/media/video/mxs_pxp.c
index 83c9c52c3b0c..017d22458a22 100644
--- a/drivers/media/video/mxs_pxp.c
+++ b/drivers/media/video/mxs_pxp.c
@@ -31,6 +31,7 @@
 #include <linux/platform_device.h>
 #include <linux/vmalloc.h>
 #include <linux/videodev2.h>
+#include <linux/delay.h>
 
 #include <media/videobuf-dma-contig.h>
 #include <media/v4l2-common.h>
@@ -671,6 +672,7 @@ static int pxp_streamon(struct file *file, void *priv, enum v4l2_buf_type t)
 
 	pxp_set_outbuf(pxp);
 	ret = videobuf_streamon(&pxp->s0_vbq);
+	msleep(20);
 
 	if (!ret && (pxp->output == 0))
 		mxsfb_cfg_pxp(1, pxp->outb_phys);
@@ -686,7 +688,9 @@ static int pxp_streamoff(struct file *file, void *priv, enum v4l2_buf_type t)
 	if ((t != V4L2_BUF_TYPE_VIDEO_OUTPUT))
 		return -EINVAL;
 
+	cancel_work_sync(&pxp->work);
 	ret = videobuf_streamoff(&pxp->s0_vbq);
+	msleep(20);
 
 	if (!ret)
 		mxsfb_cfg_pxp(0, 0);
@@ -1101,8 +1105,10 @@ static int pxp_close(struct file *file)
 {
 	struct pxps *pxp = video_get_drvdata(video_devdata(file));
 
-	if (pxp->workqueue)
+	if (pxp->workqueue) {
+		flush_workqueue(pxp->workqueue);
 		destroy_workqueue(pxp->workqueue);
+	}
 
 	videobuf_stop(&pxp->s0_vbq);
 	videobuf_mmap_free(&pxp->s0_vbq);
diff --git a/drivers/media/video/uvc/uvc_video.c b/drivers/media/video/uvc/uvc_video.c
index 01b633c73480..be99c8797dcc 100644
--- a/drivers/media/video/uvc/uvc_video.c
+++ b/drivers/media/video/uvc/uvc_video.c
@@ -707,6 +707,10 @@ static void uvc_video_complete(struct urb *urb)
 	if ((ret = usb_submit_urb(urb, GFP_ATOMIC)) < 0) {
 		uvc_printk(KERN_ERR, "Failed to resubmit video URB (%d).\n",
 			ret);
+		if (ret == -ENODEV) {
+			uvc_queue_cancel(queue, 1);
+			return;
+		}
 	}
 }
 
diff --git a/drivers/media/video/videobuf-dma-contig.c b/drivers/media/video/videobuf-dma-contig.c
index 2ac8c2421ad2..dc4f32bbd83d 100644
--- a/drivers/media/video/videobuf-dma-contig.c
+++ b/drivers/media/video/videobuf-dma-contig.c
@@ -319,7 +319,7 @@ static int __videobuf_mmap_mapper(struct videobuf_queue *q,
 
 	mem->size = PAGE_ALIGN(q->bufs[first]->bsize);
 	mem->vaddr = dma_alloc_coherent(q->dev, mem->size,
-					&mem->dma_handle, GFP_KERNEL);
+					&mem->dma_handle, GFP_DMA);
 	if (!mem->vaddr) {
 		dev_err(q->dev, "dma_alloc_coherent size %ld failed\n",
 			mem->size);
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 281b61b3b5f3..72c10192d802 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -13,6 +13,10 @@ menuconfig MISC_DEVICES
 
 if MISC_DEVICES
 
+config ANDROID_PMEM
+	bool "Android pmem allocator"
+	default y
+
 config ATMEL_PWM
 	tristate "Atmel AT32/AT91 PWM support"
 	depends on AVR32 || ARCH_AT91SAM9263 || ARCH_AT91SAM9RL || ARCH_AT91CAP9
@@ -238,6 +242,14 @@ config MXS_PERSISTENT
 	depends on ARCH_MXS
 	default y
 
+config UID_STAT
+	bool "UID based statistics tracking exported to /proc/uid_stat"
+	default n
+
+config FSL_CACHE
+	tristate "Freescale Cache manipulate driver"
+	default n
+
 source "drivers/misc/c2port/Kconfig"
 source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 03dd5ee05ce2..c435ec82d664 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -21,5 +21,8 @@ obj-$(CONFIG_HP_ILO)		+= hpilo.o
 obj-$(CONFIG_ISL29003)		+= isl29003.o
 obj-$(CONFIG_C2PORT)		+= c2port/
 obj-$(CONFIG_MXS_PERSISTENT)	+= mxs-persistent.o
+obj-$(CONFIG_ANDROID_PMEM)	+= pmem.o
+obj-$(CONFIG_UID_STAT)		+= uid_stat.o
+obj-$(CONFIG_FSL_CACHE)		+= fsl_cache.o
 obj-y				+= eeprom/
 obj-y				+= cb710/
diff --git a/drivers/mmc/core/Kconfig b/drivers/mmc/core/Kconfig
index ab37a6d9d32a..bb22ffd76ef8 100644
--- a/drivers/mmc/core/Kconfig
+++ b/drivers/mmc/core/Kconfig
@@ -3,7 +3,7 @@
 #
 
 config MMC_UNSAFE_RESUME
-	bool "Allow unsafe resume (DANGEROUS)"
+	bool "Assume MMC/SD cards are non-removable (DANGEROUS)"
 	help
 	  If you say Y here, the MMC layer will assume that all cards
 	  stayed in their respective slots during the suspend. The
@@ -14,3 +14,5 @@ config MMC_UNSAFE_RESUME
 	  This option is usually just for embedded systems which use
 	  a MMC/SD card for rootfs. Most people should say N here.
 
+	  This option sets a default which can be overridden by the
+	  module parameter "removable=0" or "removable=1".
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 50b208253440..91ba2f812d73 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -48,6 +48,22 @@ int use_spi_crc = 1;
 module_param(use_spi_crc, bool, 0);
 
 /*
+ * We normally treat cards as removed during suspend if they are not
+ * known to be on a non-removable bus, to avoid the risk of writing
+ * back data to a different card after resume.  Allow this to be
+ * overridden if necessary.
+ */
+#ifdef CONFIG_MMC_UNSAFE_RESUME
+int mmc_assume_removable;
+#else
+int mmc_assume_removable = 1;
+#endif
+module_param_named(removable, mmc_assume_removable, bool, 0644);
+MODULE_PARM_DESC(
+	removable,
+	"MMC/SD cards are removable and may be removed during suspend");
+
+/*
  * Internal function. Schedule delayed work in the MMC work queue.
  */
 static int mmc_schedule_delayed_work(struct delayed_work *work,
@@ -344,6 +360,101 @@ unsigned int mmc_align_data_size(struct mmc_card *card, unsigned int sz)
 EXPORT_SYMBOL(mmc_align_data_size);
 
 /**
+ *	mmc_host_enable - enable a host.
+ *	@host: mmc host to enable
+ *
+ *	Hosts that support power saving can use the 'enable' and 'disable'
+ *	methods to exit and enter power saving states. For more information
+ *	see comments for struct mmc_host_ops.
+ */
+int mmc_host_enable(struct mmc_host *host)
+{
+	if (!(host->caps & MMC_CAP_DISABLE))
+		return 0;
+
+	if (host->en_dis_recurs)
+		return 0;
+
+	if (host->nesting_cnt++)
+		return 0;
+
+	cancel_delayed_work_sync(&host->disable);
+
+	if (host->enabled)
+		return 0;
+
+	if (host->ops->enable) {
+		int err;
+
+		host->en_dis_recurs = 1;
+		err = host->ops->enable(host);
+		host->en_dis_recurs = 0;
+
+		if (err) {
+			pr_debug("%s: enable error %d\n",
+				 mmc_hostname(host), err);
+			return err;
+		}
+	}
+	host->enabled = 1;
+	return 0;
+}
+EXPORT_SYMBOL(mmc_host_enable);
+
+static int mmc_host_do_disable(struct mmc_host *host, int lazy)
+{
+	if (host->ops->disable) {
+		int err;
+
+		host->en_dis_recurs = 1;
+		err = host->ops->disable(host, lazy);
+		host->en_dis_recurs = 0;
+
+		if (err < 0) {
+			pr_debug("%s: disable error %d\n",
+				 mmc_hostname(host), err);
+			return err;
+		}
+		if (err > 0) {
+			unsigned long delay = msecs_to_jiffies(err);
+
+			mmc_schedule_delayed_work(&host->disable, delay);
+		}
+	}
+	host->enabled = 0;
+	return 0;
+}
+
+/**
+ *	mmc_host_disable - disable a host.
+ *	@host: mmc host to disable
+ *
+ *	Hosts that support power saving can use the 'enable' and 'disable'
+ *	methods to exit and enter power saving states. For more information
+ *	see comments for struct mmc_host_ops.
+ */
+int mmc_host_disable(struct mmc_host *host)
+{
+	int err;
+
+	if (!(host->caps & MMC_CAP_DISABLE))
+		return 0;
+
+	if (host->en_dis_recurs)
+		return 0;
+
+	if (--host->nesting_cnt)
+		return 0;
+
+	if (!host->enabled)
+		return 0;
+
+	err = mmc_host_do_disable(host, 0);
+	return err;
+}
+EXPORT_SYMBOL(mmc_host_disable);
+
+/**
  *	__mmc_claim_host - exclusively claim a host
  *	@host: mmc host to claim
  *	@abort: whether or not the operation should be aborted
@@ -366,25 +477,111 @@ int __mmc_claim_host(struct mmc_host *host, atomic_t *abort)
 	while (1) {
 		set_current_state(TASK_UNINTERRUPTIBLE);
 		stop = abort ? atomic_read(abort) : 0;
-		if (stop || !host->claimed)
+		if (stop || !host->claimed || host->claimer == current)
 			break;
 		spin_unlock_irqrestore(&host->lock, flags);
 		schedule();
 		spin_lock_irqsave(&host->lock, flags);
 	}
 	set_current_state(TASK_RUNNING);
-	if (!stop)
+	if (!stop) {
 		host->claimed = 1;
-	else
+		host->claimer = current;
+		host->claim_cnt += 1;
+	} else
 		wake_up(&host->wq);
 	spin_unlock_irqrestore(&host->lock, flags);
 	remove_wait_queue(&host->wq, &wait);
+	if (!stop)
+		mmc_host_enable(host);
 	return stop;
 }
 
 EXPORT_SYMBOL(__mmc_claim_host);
 
 /**
+ *	mmc_try_claim_host - try exclusively to claim a host
+ *	@host: mmc host to claim
+ *
+ *	Returns %1 if the host is claimed, %0 otherwise.
+ */
+int mmc_try_claim_host(struct mmc_host *host)
+{
+	int claimed_host = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&host->lock, flags);
+	if (!host->claimed || host->claimer == current) {
+		host->claimed = 1;
+		host->claimer = current;
+		host->claim_cnt += 1;
+		claimed_host = 1;
+	}
+	spin_unlock_irqrestore(&host->lock, flags);
+	return claimed_host;
+}
+EXPORT_SYMBOL(mmc_try_claim_host);
+
+static void mmc_do_release_host(struct mmc_host *host)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&host->lock, flags);
+	if (--host->claim_cnt) {
+		/* Release for nested claim */
+		spin_unlock_irqrestore(&host->lock, flags);
+	} else {
+		host->claimed = 0;
+		host->claimer = NULL;
+		spin_unlock_irqrestore(&host->lock, flags);
+		wake_up(&host->wq);
+	}
+}
+
+void mmc_host_deeper_disable(struct work_struct *work)
+{
+	struct mmc_host *host =
+		container_of(work, struct mmc_host, disable.work);
+
+	/* If the host is claimed then we do not want to disable it anymore */
+	if (!mmc_try_claim_host(host))
+		return;
+	mmc_host_do_disable(host, 1);
+	mmc_do_release_host(host);
+}
+
+/**
+ *	mmc_host_lazy_disable - lazily disable a host.
+ *	@host: mmc host to disable
+ *
+ *	Hosts that support power saving can use the 'enable' and 'disable'
+ *	methods to exit and enter power saving states. For more information
+ *	see comments for struct mmc_host_ops.
+ */
+int mmc_host_lazy_disable(struct mmc_host *host)
+{
+	if (!(host->caps & MMC_CAP_DISABLE))
+		return 0;
+
+	if (host->en_dis_recurs)
+		return 0;
+
+	if (--host->nesting_cnt)
+		return 0;
+
+	if (!host->enabled)
+		return 0;
+
+	if (host->disable_delay) {
+		mmc_schedule_delayed_work(&host->disable,
+				msecs_to_jiffies(host->disable_delay));
+		return 0;
+	} else
+		return mmc_host_do_disable(host, 1);
+}
+EXPORT_SYMBOL(mmc_host_lazy_disable);
+
+/**
  *	mmc_release_host - release a host
  *	@host: mmc host to release
  *
@@ -393,15 +590,11 @@ EXPORT_SYMBOL(__mmc_claim_host);
  */
 void mmc_release_host(struct mmc_host *host)
 {
-	unsigned long flags;
-
 	WARN_ON(!host->claimed);
 
-	spin_lock_irqsave(&host->lock, flags);
-	host->claimed = 0;
-	spin_unlock_irqrestore(&host->lock, flags);
+	mmc_host_lazy_disable(host);
 
-	wake_up(&host->wq);
+	mmc_do_release_host(host);
 }
 
 EXPORT_SYMBOL(mmc_release_host);
@@ -687,7 +880,13 @@ void mmc_set_timing(struct mmc_host *host, unsigned int timing)
  */
 static void mmc_power_up(struct mmc_host *host)
 {
-	int bit = fls(host->ocr_avail) - 1;
+	int bit;
+
+	/* If ocr is set, we use it */
+	if (host->ocr)
+		bit = ffs(host->ocr) - 1;
+	else
+		bit = fls(host->ocr_avail) - 1;
 
 	host->ios.vdd = bit;
 	if (mmc_host_is_spi(host)) {
@@ -858,6 +1057,17 @@ void mmc_rescan(struct work_struct *work)
 		container_of(work, struct mmc_host, detect.work);
 	u32 ocr;
 	int err;
+	unsigned long flags;
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (host->rescan_disable) {
+		spin_unlock_irqrestore(&host->lock, flags);
+		return;
+	}
+
+	spin_unlock_irqrestore(&host->lock, flags);
+
 
 	mmc_bus_get(host);
 
@@ -890,8 +1100,7 @@ void mmc_rescan(struct work_struct *work)
 	mmc_claim_host(host);
 
 	mmc_power_up(host);
-	sdio_go_idle(host);
-
+	sdio_reset(host);
 	mmc_go_idle(host);
 
 	mmc_send_if_cond(host, host->ocr_avail);
@@ -949,9 +1158,14 @@ void mmc_stop_host(struct mmc_host *host)
 	spin_unlock_irqrestore(&host->lock, flags);
 #endif
 
+	if (host->caps & MMC_CAP_DISABLE)
+		cancel_delayed_work(&host->disable);
 	cancel_delayed_work(&host->detect);
 	mmc_flush_scheduled_work();
 
+	/* clear pm flags now and let card drivers set them as needed */
+	host->pm_flags = 0;
+
 	mmc_bus_get(host);
 	if (host->bus_ops && !host->bus_dead) {
 		if (host->bus_ops->remove)
@@ -960,6 +1174,8 @@ void mmc_stop_host(struct mmc_host *host)
 		mmc_claim_host(host);
 		mmc_detach_bus(host);
 		mmc_release_host(host);
+		mmc_bus_put(host);
+		return;
 	}
 	mmc_bus_put(host);
 
@@ -968,6 +1184,80 @@ void mmc_stop_host(struct mmc_host *host)
 	mmc_power_off(host);
 }
 
+void mmc_power_save_host(struct mmc_host *host)
+{
+	mmc_bus_get(host);
+
+	if (!host->bus_ops || host->bus_dead || !host->bus_ops->power_restore) {
+		mmc_bus_put(host);
+		return;
+	}
+
+	if (host->bus_ops->power_save)
+		host->bus_ops->power_save(host);
+
+	mmc_bus_put(host);
+
+	mmc_power_off(host);
+}
+EXPORT_SYMBOL(mmc_power_save_host);
+
+void mmc_power_restore_host(struct mmc_host *host)
+{
+	mmc_bus_get(host);
+
+	if (!host->bus_ops || host->bus_dead || !host->bus_ops->power_restore) {
+		mmc_bus_put(host);
+		return;
+	}
+
+	mmc_power_up(host);
+	host->bus_ops->power_restore(host);
+
+	mmc_bus_put(host);
+}
+EXPORT_SYMBOL(mmc_power_restore_host);
+
+int mmc_card_awake(struct mmc_host *host)
+{
+	int err = -ENOSYS;
+
+	mmc_bus_get(host);
+
+	if (host->bus_ops && !host->bus_dead && host->bus_ops->awake)
+		err = host->bus_ops->awake(host);
+
+	mmc_bus_put(host);
+
+	return err;
+}
+EXPORT_SYMBOL(mmc_card_awake);
+
+int mmc_card_sleep(struct mmc_host *host)
+{
+	int err = -ENOSYS;
+
+	mmc_bus_get(host);
+
+	if (host->bus_ops && !host->bus_dead && host->bus_ops->awake)
+		err = host->bus_ops->sleep(host);
+
+	mmc_bus_put(host);
+
+	return err;
+}
+EXPORT_SYMBOL(mmc_card_sleep);
+
+int mmc_card_can_sleep(struct mmc_host *host)
+{
+	struct mmc_card *card = host->card;
+
+	if (card && mmc_card_mmc(card) && card->ext_csd.rev >= 3)
+		return 1;
+	return 0;
+}
+EXPORT_SYMBOL(mmc_card_can_sleep);
+
 #ifdef CONFIG_PM
 
 /**
@@ -977,27 +1267,37 @@ void mmc_stop_host(struct mmc_host *host)
  */
 int mmc_suspend_host(struct mmc_host *host, pm_message_t state)
 {
+	int err = 0;
+
+	if (host->caps & MMC_CAP_DISABLE)
+		cancel_delayed_work(&host->disable);
 	cancel_delayed_work(&host->detect);
 	mmc_flush_scheduled_work();
 
 	mmc_bus_get(host);
 	if (host->bus_ops && !host->bus_dead) {
 		if (host->bus_ops->suspend)
-			host->bus_ops->suspend(host);
-		if (!host->bus_ops->resume) {
+			err = host->bus_ops->suspend(host);
+		if (err == -ENOSYS || !host->bus_ops->resume) {
+			/*
+			 * We simply "remove" the card in this case.
+			 * It will be redetected on resume.
+			 */
 			if (host->bus_ops->remove)
 				host->bus_ops->remove(host);
-
 			mmc_claim_host(host);
 			mmc_detach_bus(host);
 			mmc_release_host(host);
+			host->pm_flags = 0;
+			err = 0;
 		}
 	}
 	mmc_bus_put(host);
 
-	mmc_power_off(host);
+	if (!err && !(host->pm_flags & MMC_PM_KEEP_POWER))
+		mmc_power_off(host);
 
-	return 0;
+	return err;
 }
 
 EXPORT_SYMBOL(mmc_suspend_host);
@@ -1008,26 +1308,75 @@ EXPORT_SYMBOL(mmc_suspend_host);
  */
 int mmc_resume_host(struct mmc_host *host)
 {
+	int err = 0;
+
 	mmc_bus_get(host);
 	if (host->bus_ops && !host->bus_dead) {
-		mmc_power_up(host);
-		mmc_select_voltage(host, host->ocr);
+		if (!(host->pm_flags & MMC_PM_KEEP_POWER)) {
+			mmc_power_up(host);
+			mmc_select_voltage(host, host->ocr);
+		}
 		BUG_ON(!host->bus_ops->resume);
-		host->bus_ops->resume(host);
+		err = host->bus_ops->resume(host);
+		if (err) {
+			printk(KERN_WARNING "%s: error %d during resume "
+					    "(card was removed?)\n",
+					    mmc_hostname(host), err);
+			err = 0;
+		}
 	}
 	mmc_bus_put(host);
 
-	/*
-	 * We add a slight delay here so that resume can progress
-	 * in parallel.
-	 */
-	mmc_detect_change(host, 1);
-
-	return 0;
+	return err;
 }
-
 EXPORT_SYMBOL(mmc_resume_host);
 
+/* Do the card removal on suspend if card is assumed removeable
+ * Do that in pm notifier while userspace isn't yet frozen, so we will be able
+   to sync the card.
+*/
+int mmc_pm_notify(struct notifier_block *notify_block,
+					unsigned long mode, void *unused)
+{
+	struct mmc_host *host = container_of(
+		notify_block, struct mmc_host, pm_notify);
+	unsigned long flags;
+
+
+	switch (mode) {
+	case PM_HIBERNATION_PREPARE:
+	case PM_SUSPEND_PREPARE:
+
+		spin_lock_irqsave(&host->lock, flags);
+		host->rescan_disable = 1;
+		spin_unlock_irqrestore(&host->lock, flags);
+		cancel_delayed_work_sync(&host->detect);
+
+		if (!host->bus_ops || host->bus_ops->suspend)
+			break;
+
+		mmc_claim_host(host);
+
+		if (host->bus_ops->remove)
+			host->bus_ops->remove(host);
+
+		mmc_detach_bus(host);
+		mmc_release_host(host);
+		host->pm_flags = 0;
+		break;
+
+	case PM_POST_SUSPEND:
+	case PM_POST_HIBERNATION:
+
+		spin_lock_irqsave(&host->lock, flags);
+		host->rescan_disable = 0;
+		spin_unlock_irqrestore(&host->lock, flags);
+		mmc_detect_change(host, 0);
+
+	}
+
+	return 0;
+}
 #endif
 
 static int __init mmc_init(void)
diff --git a/drivers/mmc/core/core.h b/drivers/mmc/core/core.h
index c819effa1032..a811c52a1659 100644
--- a/drivers/mmc/core/core.h
+++ b/drivers/mmc/core/core.h
@@ -16,10 +16,14 @@
 #define MMC_CMD_RETRIES        3
 
 struct mmc_bus_ops {
+	int (*awake)(struct mmc_host *);
+	int (*sleep)(struct mmc_host *);
 	void (*remove)(struct mmc_host *);
 	void (*detect)(struct mmc_host *);
-	void (*suspend)(struct mmc_host *);
-	void (*resume)(struct mmc_host *);
+	int (*suspend)(struct mmc_host *);
+	int (*resume)(struct mmc_host *);
+	void (*power_save)(struct mmc_host *);
+	void (*power_restore)(struct mmc_host *);
 };
 
 void mmc_attach_bus(struct mmc_host *host, const struct mmc_bus_ops *ops);
@@ -50,7 +54,9 @@ int mmc_attach_mmc(struct mmc_host *host, u32 ocr);
 int mmc_attach_sd(struct mmc_host *host, u32 ocr);
 int mmc_attach_sdio(struct mmc_host *host, u32 ocr);
 
+/* Module parameters */
 extern int use_spi_crc;
+extern int mmc_assume_removable;
 
 /* Debugfs information for hosts and cards */
 void mmc_add_host_debugfs(struct mmc_host *host);
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index 5e945e64ead7..0efe631e50ca 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -16,6 +16,8 @@
 #include <linux/idr.h>
 #include <linux/pagemap.h>
 #include <linux/leds.h>
+#include <linux/slab.h>
+#include <linux/suspend.h>
 
 #include <linux/mmc/host.h>
 
@@ -83,6 +85,8 @@ struct mmc_host *mmc_alloc_host(int extra, struct device *dev)
 	spin_lock_init(&host->lock);
 	init_waitqueue_head(&host->wq);
 	INIT_DELAYED_WORK(&host->detect, mmc_rescan);
+	INIT_DELAYED_WORK_DEFERRABLE(&host->disable, mmc_host_deeper_disable);
+	host->pm_notify.notifier_call = mmc_pm_notify;
 
 	/*
 	 * By default, hosts do not support SGIO or large requests.
@@ -131,6 +135,7 @@ int mmc_add_host(struct mmc_host *host)
 #endif
 
 	mmc_start_host(host);
+	register_pm_notifier(&host->pm_notify);
 
 	return 0;
 }
@@ -147,6 +152,7 @@ EXPORT_SYMBOL(mmc_add_host);
  */
 void mmc_remove_host(struct mmc_host *host)
 {
+	unregister_pm_notifier(&host->pm_notify);
 	mmc_stop_host(host);
 
 #ifdef CONFIG_DEBUG_FS
diff --git a/drivers/mmc/core/host.h b/drivers/mmc/core/host.h
index c2dc3d2d9f9a..8c87e1109a34 100644
--- a/drivers/mmc/core/host.h
+++ b/drivers/mmc/core/host.h
@@ -14,5 +14,7 @@
 int mmc_register_host_class(void);
 void mmc_unregister_host_class(void);
 
+void mmc_host_deeper_disable(struct work_struct *work);
+
 #endif
 
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index e207dcf9e754..2338c761c74f 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -160,7 +160,6 @@ static int mmc_read_ext_csd(struct mmc_card *card)
 {
 	int err;
 	u8 *ext_csd;
-	unsigned int ext_csd_struct;
 
 	BUG_ON(!card);
 
@@ -207,16 +206,16 @@ static int mmc_read_ext_csd(struct mmc_card *card)
 		goto out;
 	}
 
-	ext_csd_struct = ext_csd[EXT_CSD_REV];
-	if (ext_csd_struct > 5) {
+	card->ext_csd.rev = ext_csd[EXT_CSD_REV];
+	if (card->ext_csd.rev > 3) {
 		printk(KERN_ERR "%s: unrecognised EXT_CSD structure "
 			"version %d\n", mmc_hostname(card->host),
-			ext_csd_struct);
+			card->ext_csd.rev);
 		err = -EINVAL;
 		goto out;
 	}
 
-	if (ext_csd_struct >= 2) {
+	if (card->ext_csd.rev >= 2) {
 		card->ext_csd.sectors =
 			ext_csd[EXT_CSD_SEC_CNT + 0] << 0 |
 			ext_csd[EXT_CSD_SEC_CNT + 1] << 8 |
@@ -226,7 +225,17 @@ static int mmc_read_ext_csd(struct mmc_card *card)
 			mmc_card_set_blockaddr(card);
 	}
 
+	card->ext_csd.boot_info = ext_csd[EXT_CSD_BOOT_INFO];
+	card->ext_csd.boot_size_mult = ext_csd[EXT_CSD_BOOT_SIZE_MULT];
+	card->ext_csd.boot_config = ext_csd[EXT_CSD_BOOT_CONFIG];
+	card->ext_csd.boot_bus_width = ext_csd[EXT_CSD_BOOT_BUS_WIDTH];
+	card->ext_csd.card_type = ext_csd[EXT_CSD_CARD_TYPE];
+
 	switch (ext_csd[EXT_CSD_CARD_TYPE]) {
+	case EXT_CSD_CARD_TYPE_DDR_52 | EXT_CSD_CARD_TYPE_52
+		| EXT_CSD_CARD_TYPE_26:
+		card->ext_csd.hs_max_dtr = 52000000;
+		break;
 	case EXT_CSD_CARD_TYPE_52 | EXT_CSD_CARD_TYPE_26:
 		card->ext_csd.hs_max_dtr = 52000000;
 		break;
@@ -238,15 +247,301 @@ static int mmc_read_ext_csd(struct mmc_card *card)
 		printk(KERN_WARNING "%s: card is mmc v4 but doesn't "
 			"support any high-speed modes.\n",
 			mmc_hostname(card->host));
+		printk(KERN_WARNING "%s: card type is 0x%x\n",
+			mmc_hostname(card->host), ext_csd[EXT_CSD_CARD_TYPE]);
 		goto out;
 	}
 
+	if (card->ext_csd.rev >= 3) {
+		u8 sa_shift = ext_csd[EXT_CSD_S_A_TIMEOUT];
+
+		/* Sleep / awake timeout in 100ns units */
+		if (sa_shift > 0 && sa_shift <= 0x17)
+			card->ext_csd.sa_timeout =
+					1 << ext_csd[EXT_CSD_S_A_TIMEOUT];
+	}
+
 out:
 	kfree(ext_csd);
 
 	return err;
 }
 
+/* configure the boot partitions */
+static ssize_t
+setup_boot_partitions(struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	int err, busy = 0;
+	u32 part, new_part;
+	u8 *ext_csd, boot_config;
+	struct mmc_command cmd;
+	struct mmc_card *card = container_of(dev, struct mmc_card, dev);
+
+	BUG_ON(!card);
+
+	sscanf(buf, "%d\n", &part);
+
+	if (card->csd.mmca_vsn < CSD_SPEC_VER_4) {
+		printk(KERN_ERR "%s: invalid mmc version"
+			" mmc version is below version 4!)\n",
+			mmc_hostname(card->host));
+		return -EINVAL;
+	}
+
+	/* it's a normal SD/MMC but user request to configure boot partition */
+	if (card->ext_csd.boot_size_mult <= 0) {
+		printk(KERN_ERR "%s: this is a normal SD/MMC card"
+			" but you request to access boot partition!\n",
+			mmc_hostname(card->host));
+		return -EINVAL;
+	}
+
+	ext_csd = kmalloc(512, GFP_KERNEL);
+	if (!ext_csd) {
+		printk(KERN_ERR "%s: could not allocate a buffer to "
+			"receive the ext_csd.\n", mmc_hostname(card->host));
+		return -ENOMEM;
+	}
+
+	mmc_claim_host(card->host);
+	err = mmc_send_ext_csd(card, ext_csd);
+	if (err) {
+		printk(KERN_ERR "%s: unable to read EXT_CSD.\n",
+			mmc_hostname(card->host));
+		goto err_rtn;
+	}
+
+	/* enable the boot partition in boot mode */
+	/* boot enable be -
+	 * 0x00 - disable boot enable.
+	 * 0x08 - boot partition 1 is enabled for boot.
+	 * 0x10 - boot partition 2 is enabled for boot.
+	 * 0x38 - User area is enabled for boot.
+	 */
+	switch (part & EXT_CSD_BOOT_PARTITION_ENABLE_MASK) {
+	case 0:
+		boot_config = (ext_csd[EXT_CSD_BOOT_CONFIG]
+				& ~EXT_CSD_BOOT_PARTITION_ENABLE_MASK
+				& ~EXT_CSD_BOOT_ACK_ENABLE);
+		break;
+	case EXT_CSD_BOOT_PARTITION_PART1:
+		boot_config = ((ext_csd[EXT_CSD_BOOT_CONFIG]
+				& ~EXT_CSD_BOOT_PARTITION_ENABLE_MASK)
+				| EXT_CSD_BOOT_PARTITION_PART1
+				| EXT_CSD_BOOT_ACK_ENABLE);
+		break;
+	case EXT_CSD_BOOT_PARTITION_PART2:
+		boot_config = ((ext_csd[EXT_CSD_BOOT_CONFIG]
+				& ~EXT_CSD_BOOT_PARTITION_ENABLE_MASK)
+				| EXT_CSD_BOOT_PARTITION_PART2
+				| EXT_CSD_BOOT_ACK_ENABLE);
+		break;
+	case EXT_CSD_BOOT_PARTITION_ENABLE_MASK:
+		boot_config = ((ext_csd[EXT_CSD_BOOT_CONFIG]
+				| EXT_CSD_BOOT_PARTITION_ENABLE_MASK)
+				& ~EXT_CSD_BOOT_ACK_ENABLE);
+		break;
+	default:
+		printk(KERN_ERR "%s: wrong boot config parameter"
+			" 00 (disable boot), 08 (enable boot1),"
+			"16 (enable boot2), 56 (User area)\n",
+			mmc_hostname(card->host));
+		err = -EINVAL;
+		goto err_rtn;
+	}
+
+	/* switch the partitions that used to be accessed in OS layer */
+	/* partition must be -
+	 * 0 - user area
+	 * 1 - boot partition 1
+	 * 2 - boot partition 2
+	 */
+	if ((part & EXT_CSD_BOOT_PARTITION_ACCESS_MASK) > 2) {
+		printk(KERN_ERR "%s: wrong partition id"
+			" 0 (user area), 1 (boot1), 2 (boot2)\n",
+			mmc_hostname(card->host));
+		err = -EINVAL;
+		goto err_rtn;
+	}
+
+	/* Send SWITCH command to change partition for access */
+	boot_config &= ~EXT_CSD_BOOT_PARTITION_ACCESS_MASK;
+	boot_config |= (part & EXT_CSD_BOOT_PARTITION_ACCESS_MASK);
+	err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			EXT_CSD_BOOT_CONFIG, boot_config);
+	if (err) {
+		printk(KERN_ERR "%s: fail to send SWITCH command"
+				" to card to swich partition for access!\n",
+			mmc_hostname(card->host));
+		goto err_rtn;
+	}
+
+	/* waiting for the card to finish the busy state */
+	do {
+		memset(&cmd, 0, sizeof(struct mmc_command));
+
+		cmd.opcode = MMC_SEND_STATUS;
+		cmd.arg = card->rca << 16;
+		cmd.flags = MMC_RSP_R1 | MMC_CMD_AC;
+
+		err = mmc_wait_for_cmd(card->host, &cmd, 0);
+		if (err || busy > 100) {
+			printk(KERN_ERR "%s: failed to wait for"
+				"the busy state to end.\n",
+				mmc_hostname(card->host));
+			break;
+		}
+
+		if (!busy && !(cmd.resp[0] & R1_READY_FOR_DATA)) {
+			printk(KERN_INFO "%s: card is in busy state"
+				"pls wait for busy state to end.\n",
+				mmc_hostname(card->host));
+		}
+		busy++;
+	} while (!(cmd.resp[0] & R1_READY_FOR_DATA));
+
+	/* Now check whether it works */
+	err = mmc_send_ext_csd(card, ext_csd);
+	if (err) {
+		printk(KERN_ERR "%s: %d unable to re-read EXT_CSD.\n",
+			mmc_hostname(card->host), err);
+		goto err_rtn;
+	}
+
+	new_part = ext_csd[EXT_CSD_BOOT_CONFIG] &
+		EXT_CSD_BOOT_PARTITION_ACCESS_MASK;
+	if ((part & EXT_CSD_BOOT_PARTITION_ACCESS_MASK) != new_part) {
+		printk(KERN_ERR "%s: after SWITCH, current part id %d is not"
+				" same as requested partition %d!\n",
+			mmc_hostname(card->host), new_part, part);
+		goto err_rtn;
+	}
+	card->ext_csd.boot_config = ext_csd[EXT_CSD_BOOT_CONFIG];
+
+err_rtn:
+	mmc_release_host(card->host);
+	kfree(ext_csd);
+	if (err)
+		return err;
+	else
+		return count;
+}
+
+/* configure the boot bus */
+static ssize_t
+setup_boot_bus(struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	int err, busy = 0;
+	u32 boot_bus, new_bus;
+	u8 *ext_csd;
+	struct mmc_command cmd;
+	struct mmc_card *card = container_of(dev, struct mmc_card, dev);
+
+	BUG_ON(!card);
+
+	sscanf(buf, "%d\n", &boot_bus);
+
+	if (card->csd.mmca_vsn < CSD_SPEC_VER_4) {
+		printk(KERN_ERR "%s: invalid mmc version"
+			" mmc version is below version 4!)\n",
+			mmc_hostname(card->host));
+		return -EINVAL;
+	}
+
+	/* it's a normal SD/MMC but user request to configure boot bus */
+	if (card->ext_csd.boot_size_mult <= 0) {
+		printk(KERN_ERR "%s: this is a normal SD/MMC card"
+			" but you request to configure boot bus !\n",
+			mmc_hostname(card->host));
+		return -EINVAL;
+	}
+
+	ext_csd = kmalloc(512, GFP_KERNEL);
+	if (!ext_csd) {
+		printk(KERN_ERR "%s: could not allocate a buffer to "
+			"receive the ext_csd.\n", mmc_hostname(card->host));
+		return -ENOMEM;
+	}
+
+	mmc_claim_host(card->host);
+	err = mmc_send_ext_csd(card, ext_csd);
+	if (err) {
+		printk(KERN_ERR "%s: unable to read EXT_CSD.\n",
+			mmc_hostname(card->host));
+		goto err_rtn;
+	}
+
+	/* Configure the boot bus width when boot partition is enabled */
+	if (((boot_bus & EXT_CSD_BOOT_BUS_WIDTH_MODE_MASK) >> 3) > 2
+			|| (boot_bus & EXT_CSD_BOOT_BUS_WIDTH_WIDTH_MASK) > 2
+			|| (boot_bus & ~EXT_CSD_BOOT_BUS_WIDTH_MASK) > 0) {
+		printk(KERN_ERR "%s: Invalid inputs!\n",
+			mmc_hostname(card->host));
+		err = -EINVAL;
+		goto err_rtn;
+	}
+
+	err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
+			EXT_CSD_BOOT_BUS_WIDTH, boot_bus);
+	if (err) {
+		printk(KERN_ERR "%s: fail to send SWITCH command to "
+				"card to swich partition for access!\n",
+			mmc_hostname(card->host));
+		goto err_rtn;
+	}
+
+	/* waiting for the card to finish the busy state */
+	do {
+		memset(&cmd, 0, sizeof(struct mmc_command));
+
+		cmd.opcode = MMC_SEND_STATUS;
+		cmd.arg = card->rca << 16;
+		cmd.flags = MMC_RSP_R1 | MMC_CMD_AC;
+
+		err = mmc_wait_for_cmd(card->host, &cmd, 0);
+		if (err || busy > 100) {
+			printk(KERN_ERR "%s: failed to wait for"
+				"the busy state to end.\n",
+				mmc_hostname(card->host));
+			break;
+		}
+
+		if (!busy && !(cmd.resp[0] & R1_READY_FOR_DATA)) {
+			printk(KERN_INFO "%s: card is in busy state"
+				"pls wait for busy state to end.\n",
+				mmc_hostname(card->host));
+		}
+		busy++;
+	} while (!(cmd.resp[0] & R1_READY_FOR_DATA));
+
+	/* Now check whether it works */
+	err = mmc_send_ext_csd(card, ext_csd);
+	if (err) {
+		printk(KERN_ERR "%s: %d unable to re-read EXT_CSD.\n",
+			mmc_hostname(card->host), err);
+		goto err_rtn;
+	}
+
+	new_bus = ext_csd[EXT_CSD_BOOT_BUS_WIDTH];
+	if (boot_bus  != new_bus) {
+		printk(KERN_ERR "%s: after SWITCH, current boot bus mode %d"
+				" is not same as requested bus mode %d!\n",
+			mmc_hostname(card->host), new_bus, boot_bus);
+		goto err_rtn;
+	}
+	card->ext_csd.boot_bus_width = ext_csd[EXT_CSD_BOOT_BUS_WIDTH];
+
+err_rtn:
+	mmc_release_host(card->host);
+	kfree(ext_csd);
+	if (err)
+		return err;
+	else
+		return count;
+}
+
 MMC_DEV_ATTR(cid, "%08x%08x%08x%08x\n", card->raw_cid[0], card->raw_cid[1],
 	card->raw_cid[2], card->raw_cid[3]);
 MMC_DEV_ATTR(csd, "%08x%08x%08x%08x\n", card->raw_csd[0], card->raw_csd[1],
@@ -258,6 +553,12 @@ MMC_DEV_ATTR(manfid, "0x%06x\n", card->cid.manfid);
 MMC_DEV_ATTR(name, "%s\n", card->cid.prod_name);
 MMC_DEV_ATTR(oemid, "0x%04x\n", card->cid.oemid);
 MMC_DEV_ATTR(serial, "0x%08x\n", card->cid.serial);
+MMC_DEV_ATTR(boot_info, "boot_info:0x%02x; boot_size:%04dKB;"
+		" boot_partition:0x%02x; boot_bus:0x%02x\n",
+	card->ext_csd.boot_info, card->ext_csd.boot_size_mult * 128,
+	card->ext_csd.boot_config, card->ext_csd.boot_bus_width);
+DEVICE_ATTR(boot_config, S_IWUGO, NULL, setup_boot_partitions);
+DEVICE_ATTR(boot_bus_config, S_IWUGO, NULL, setup_boot_bus);
 
 static struct attribute *mmc_std_attrs[] = {
 	&dev_attr_cid.attr,
@@ -269,6 +570,9 @@ static struct attribute *mmc_std_attrs[] = {
 	&dev_attr_name.attr,
 	&dev_attr_oemid.attr,
 	&dev_attr_serial.attr,
+	&dev_attr_boot_info.attr,
+	&dev_attr_boot_config.attr,
+	&dev_attr_boot_bus_config.attr,
 	NULL,
 };
 
@@ -434,10 +738,21 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	 * Activate wide bus (if supported).
 	 */
 	if ((card->csd.mmca_vsn >= CSD_SPEC_VER_4) &&
-	    (host->caps & (MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA))) {
+		(host->caps & (MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA
+			       | MMC_CAP_DATA_DDR))) {
 		unsigned ext_csd_bit, bus_width;
 
-		if (host->caps & MMC_CAP_8_BIT_DATA) {
+		if ((host->caps & MMC_CAP_8_BIT_DATA) &&
+				(host->caps & MMC_CAP_DATA_DDR) &&
+				(card->ext_csd.card_type & MMC_DDR_MODE_MASK)) {
+			ext_csd_bit = EXT_CSD_BUS_WIDTH_8_DDR;
+			bus_width = MMC_BUS_WIDTH_8 | MMC_BUS_WIDTH_DDR;
+		} else if ((host->caps & MMC_CAP_4_BIT_DATA) &&
+				(host->caps & MMC_CAP_DATA_DDR) &&
+				(card->ext_csd.card_type & MMC_DDR_MODE_MASK)) {
+			ext_csd_bit = EXT_CSD_BUS_WIDTH_4_DDR;
+			bus_width = MMC_BUS_WIDTH_4 | MMC_BUS_WIDTH_DDR;
+		} else if (host->caps & MMC_CAP_8_BIT_DATA) {
 			ext_csd_bit = EXT_CSD_BUS_WIDTH_8;
 			bus_width = MMC_BUS_WIDTH_8;
 		} else {
@@ -507,12 +822,10 @@ static void mmc_detect(struct mmc_host *host)
 	}
 }
 
-#ifdef CONFIG_MMC_UNSAFE_RESUME
-
 /*
  * Suspend callback from host.
  */
-static void mmc_suspend(struct mmc_host *host)
+static int mmc_suspend(struct mmc_host *host)
 {
 	BUG_ON(!host);
 	BUG_ON(!host->card);
@@ -522,6 +835,8 @@ static void mmc_suspend(struct mmc_host *host)
 		mmc_deselect_cards(host);
 	host->card->state &= ~MMC_STATE_HIGHSPEED;
 	mmc_release_host(host);
+
+	return 0;
 }
 
 /*
@@ -530,7 +845,7 @@ static void mmc_suspend(struct mmc_host *host)
  * This function tries to determine if the same card is still present
  * and, if so, restore all state to it.
  */
-static void mmc_resume(struct mmc_host *host)
+static int mmc_resume(struct mmc_host *host)
 {
 	int err;
 
@@ -541,30 +856,78 @@ static void mmc_resume(struct mmc_host *host)
 	err = mmc_init_card(host, host->ocr, host->card);
 	mmc_release_host(host);
 
-	if (err) {
-		mmc_remove(host);
+	return err;
+}
 
-		mmc_claim_host(host);
-		mmc_detach_bus(host);
-		mmc_release_host(host);
+static void mmc_power_restore(struct mmc_host *host)
+{
+	host->card->state &= ~MMC_STATE_HIGHSPEED;
+	mmc_claim_host(host);
+	mmc_init_card(host, host->ocr, host->card);
+	mmc_release_host(host);
+}
+
+static int mmc_sleep(struct mmc_host *host)
+{
+	struct mmc_card *card = host->card;
+	int err = -ENOSYS;
+
+	if (card && card->ext_csd.rev >= 3) {
+		err = mmc_card_sleepawake(host, 1);
+		if (err < 0)
+			pr_debug("%s: Error %d while putting card into sleep",
+				 mmc_hostname(host), err);
 	}
 
+	return err;
 }
 
-#else
-
-#define mmc_suspend NULL
-#define mmc_resume NULL
+static int mmc_awake(struct mmc_host *host)
+{
+	struct mmc_card *card = host->card;
+	int err = -ENOSYS;
+
+	if (card && card->ext_csd.rev >= 3) {
+		err = mmc_card_sleepawake(host, 0);
+		if (err < 0)
+			pr_debug("%s: Error %d while awaking sleeping card",
+				 mmc_hostname(host), err);
+	}
 
-#endif
+	return err;
+}
 
 static const struct mmc_bus_ops mmc_ops = {
+	.awake = mmc_awake,
+	.sleep = mmc_sleep,
+	.remove = mmc_remove,
+	.detect = mmc_detect,
+	.suspend = NULL,
+	.resume = NULL,
+	.power_restore = mmc_power_restore,
+};
+
+static const struct mmc_bus_ops mmc_ops_unsafe = {
+	.awake = mmc_awake,
+	.sleep = mmc_sleep,
 	.remove = mmc_remove,
 	.detect = mmc_detect,
 	.suspend = mmc_suspend,
 	.resume = mmc_resume,
+	.power_restore = mmc_power_restore,
 };
 
+static void mmc_attach_bus_ops(struct mmc_host *host)
+{
+	const struct mmc_bus_ops *bus_ops;
+
+	if (host->caps & MMC_CAP_NONREMOVABLE || !mmc_assume_removable)
+		bus_ops = &mmc_ops_unsafe;
+	else
+		bus_ops = &mmc_ops;
+	mmc_attach_bus(host, bus_ops);
+}
+
 /*
  * Starting point for MMC card init.
  */
@@ -575,7 +938,7 @@ int mmc_attach_mmc(struct mmc_host *host, u32 ocr)
 	BUG_ON(!host);
 	WARN_ON(!host->claimed);
 
-	mmc_attach_bus(host, &mmc_ops);
+	mmc_attach_bus_ops(host);
 
 	/*
 	 * We need to get OCR a different way for SPI.
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 34ce2703d29a..355c6042cf65 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -57,6 +57,42 @@ int mmc_deselect_cards(struct mmc_host *host)
 	return _mmc_select_card(host, NULL);
 }
 
+int mmc_card_sleepawake(struct mmc_host *host, int sleep)
+{
+	struct mmc_command cmd;
+	struct mmc_card *card = host->card;
+	int err;
+
+	if (sleep)
+		mmc_deselect_cards(host);
+
+	memset(&cmd, 0, sizeof(struct mmc_command));
+
+	cmd.opcode = MMC_SLEEP_AWAKE;
+	cmd.arg = card->rca << 16;
+	if (sleep)
+		cmd.arg |= 1 << 15;
+
+	cmd.flags = MMC_RSP_R1B | MMC_CMD_AC;
+	err = mmc_wait_for_cmd(host, &cmd, 0);
+	if (err)
+		return err;
+
+	/*
+	 * If the host does not wait while the card signals busy, then we will
+	 * will have to wait the sleep/awake timeout.  Note, we cannot use the
+	 * SEND_STATUS command to poll the status because that command (and most
+	 * others) is invalid while the card sleeps.
+	 */
+	if (!(host->caps & MMC_CAP_WAIT_WHILE_BUSY))
+		mmc_delay(DIV_ROUND_UP(card->ext_csd.sa_timeout, 10000));
+
+	if (!sleep)
+		err = mmc_select_card(card);
+
+	return err;
+}
+
 int mmc_go_idle(struct mmc_host *host)
 {
 	int err;
diff --git a/drivers/mmc/core/mmc_ops.h b/drivers/mmc/core/mmc_ops.h
index 17854bf7cf0d..653eb8e84178 100644
--- a/drivers/mmc/core/mmc_ops.h
+++ b/drivers/mmc/core/mmc_ops.h
@@ -25,6 +25,7 @@ int mmc_send_status(struct mmc_card *card, u32 *status);
 int mmc_send_cid(struct mmc_host *host, u32 *cid);
 int mmc_spi_read_ocr(struct mmc_host *host, int highcap, u32 *ocrp);
 int mmc_spi_set_crc(struct mmc_host *host, int use_crc);
+int mmc_card_sleepawake(struct mmc_host *host, int sleep);
 
 #endif
 
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index cd81c395e164..4a73e34f9200 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -561,12 +561,10 @@ static void mmc_sd_detect(struct mmc_host *host)
 	}
 }
 
-#ifdef CONFIG_MMC_UNSAFE_RESUME
-
 /*
  * Suspend callback from host.
  */
-static void mmc_sd_suspend(struct mmc_host *host)
+static int mmc_sd_suspend(struct mmc_host *host)
 {
 	BUG_ON(!host);
 	BUG_ON(!host->card);
@@ -576,6 +574,8 @@ static void mmc_sd_suspend(struct mmc_host *host)
 		mmc_deselect_cards(host);
 	host->card->state &= ~MMC_STATE_HIGHSPEED;
 	mmc_release_host(host);
+
+	return 0;
 }
 
 /*
@@ -584,7 +584,7 @@ static void mmc_sd_suspend(struct mmc_host *host)
  * This function tries to determine if the same card is still present
  * and, if so, restore all state to it.
  */
-static void mmc_sd_resume(struct mmc_host *host)
+static int mmc_sd_resume(struct mmc_host *host)
 {
 	int err;
 
@@ -595,30 +595,44 @@ static void mmc_sd_resume(struct mmc_host *host)
 	err = mmc_sd_init_card(host, host->ocr, host->card);
 	mmc_release_host(host);
 
-	if (err) {
-		mmc_sd_remove(host);
-
-		mmc_claim_host(host);
-		mmc_detach_bus(host);
-		mmc_release_host(host);
-	}
-
+	return err;
 }
 
-#else
-
-#define mmc_sd_suspend NULL
-#define mmc_sd_resume NULL
-
-#endif
+static void mmc_sd_power_restore(struct mmc_host *host)
+{
+	host->card->state &= ~MMC_STATE_HIGHSPEED;
+	mmc_claim_host(host);
+	mmc_sd_init_card(host, host->ocr, host->card);
+	mmc_release_host(host);
+}
 
 static const struct mmc_bus_ops mmc_sd_ops = {
 	.remove = mmc_sd_remove,
 	.detect = mmc_sd_detect,
+	.suspend = NULL,
+	.resume = NULL,
+	.power_restore = mmc_sd_power_restore,
+};
+
+static const struct mmc_bus_ops mmc_sd_ops_unsafe = {
+	.remove = mmc_sd_remove,
+	.detect = mmc_sd_detect,
 	.suspend = mmc_sd_suspend,
 	.resume = mmc_sd_resume,
+	.power_restore = mmc_sd_power_restore,
 };
 
+static void mmc_sd_attach_bus_ops(struct mmc_host *host)
+{
+	const struct mmc_bus_ops *bus_ops;
+
+	if (host->caps & MMC_CAP_NONREMOVABLE || !mmc_assume_removable)
+		bus_ops = &mmc_sd_ops_unsafe;
+	else
+		bus_ops = &mmc_sd_ops;
+	mmc_attach_bus(host, bus_ops);
+}
+
 /*
  * Starting point for SD card init.
  */
@@ -629,7 +643,7 @@ int mmc_attach_sd(struct mmc_host *host, u32 ocr)
 	BUG_ON(!host);
 	WARN_ON(!host->claimed);
 
-	mmc_attach_bus(host, &mmc_sd_ops);
+	mmc_sd_attach_bus_ops(host);
 
 	/*
 	 * We need to get OCR a different way for SPI.
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index fb99ccff9080..7f3093dd2b49 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -195,6 +195,135 @@ static int sdio_enable_hs(struct mmc_card *card)
 }
 
 /*
+ * Handle the detection and initialisation of a card.
+ *
+ * In the case of a resume, "oldcard" will contain the card
+ * we're trying to reinitialise.
+ */
+static int mmc_sdio_init_card(struct mmc_host *host, u32 ocr,
+			      struct mmc_card *oldcard)
+{
+	struct mmc_card *card;
+	int err;
+
+	BUG_ON(!host);
+	WARN_ON(!host->claimed);
+
+	/*
+	 * Inform the card of the voltage
+	 */
+	err = mmc_send_io_op_cond(host, host->ocr, &ocr);
+	if (err)
+		goto err;
+
+	/*
+	 * For SPI, enable CRC as appropriate.
+	 */
+	if (mmc_host_is_spi(host)) {
+		err = mmc_spi_set_crc(host, use_spi_crc);
+		if (err)
+			goto err;
+	}
+
+	/*
+	 * Allocate card structure.
+	 */
+	card = mmc_alloc_card(host, NULL);
+	if (IS_ERR(card)) {
+		err = PTR_ERR(card);
+		goto err;
+	}
+
+	card->type = MMC_TYPE_SDIO;
+
+	/*
+	 * For native busses:  set card RCA and quit open drain mode.
+	 */
+	if (!mmc_host_is_spi(host)) {
+		err = mmc_send_relative_addr(host, &card->rca);
+		if (err)
+			goto remove;
+
+		mmc_set_bus_mode(host, MMC_BUSMODE_PUSHPULL);
+	}
+
+	/*
+	 * Select card, as all following commands rely on that.
+	 */
+	if (!mmc_host_is_spi(host)) {
+		err = mmc_select_card(card);
+		if (err)
+			goto remove;
+	}
+
+	/*
+	 * Read the common registers.
+	 */
+	err = sdio_read_cccr(card);
+	if (err)
+		goto remove;
+
+	/*
+	 * Read the common CIS tuples.
+	 */
+	err = sdio_read_common_cis(card);
+	if (err)
+		goto remove;
+
+	if (oldcard) {
+		int same = (card->cis.vendor == oldcard->cis.vendor &&
+			    card->cis.device == oldcard->cis.device);
+		mmc_remove_card(card);
+		if (!same) {
+			err = -ENOENT;
+			goto err;
+		}
+		card = oldcard;
+		return 0;
+	}
+
+	/*
+	 * Switch to high-speed (if supported).
+	 */
+	err = sdio_enable_hs(card);
+	if (err)
+		goto remove;
+
+	/*
+	 * Change to the card's maximum speed.
+	 */
+	if (mmc_card_highspeed(card)) {
+		/*
+		 * The SDIO specification doesn't mention how
+		 * the CIS transfer speed register relates to
+		 * high-speed, but it seems that 50 MHz is
+		 * mandatory.
+		 */
+		mmc_set_clock(host, 50000000);
+	} else {
+		mmc_set_clock(host, card->cis.max_dtr);
+	}
+
+	/*
+	 * Switch to wider bus (if supported).
+	 */
+	err = sdio_enable_wide(card);
+	if (err)
+		goto remove;
+
+	if (!oldcard)
+		host->card = card;
+	return 0;
+
+remove:
+	if (!oldcard)
+		mmc_remove_card(card);
+
+err:
+	return err;
+}
+
+/*
  * Host is being removed. Free up the current card.
  */
 static void mmc_sdio_remove(struct mmc_host *host)
@@ -243,10 +372,77 @@ static void mmc_sdio_detect(struct mmc_host *host)
 	}
 }
 
+/*
+ * SDIO suspend.  We need to suspend all functions separately.
+ * Therefore all registered functions must have drivers with suspend
+ * and resume methods.  Failing that we simply remove the whole card.
+ */
+static int mmc_sdio_suspend(struct mmc_host *host)
+{
+	int i, err = 0;
+
+	for (i = 0; i < host->card->sdio_funcs; i++) {
+		struct sdio_func *func = host->card->sdio_func[i];
+		if (func && sdio_func_present(func) && func->dev.driver) {
+			const struct dev_pm_ops *pmops = func->dev.driver->pm;
+			if (!pmops || !pmops->suspend || !pmops->resume) {
+				/* force removal of entire card in that case */
+				err = -ENOSYS;
+			} else
+				err = pmops->suspend(&func->dev);
+			if (err)
+				break;
+		}
+	}
+	while (err && --i >= 0) {
+		struct sdio_func *func = host->card->sdio_func[i];
+		if (func && sdio_func_present(func) && func->dev.driver) {
+			const struct dev_pm_ops *pmops = func->dev.driver->pm;
+			pmops->resume(&func->dev);
+		}
+	}
+
+	return err;
+}
+
+static int mmc_sdio_resume(struct mmc_host *host)
+{
+	int i, err;
+
+	BUG_ON(!host);
+	BUG_ON(!host->card);
+
+	/* Basic card reinitialization. */
+	mmc_claim_host(host);
+	err = mmc_sdio_init_card(host, host->ocr, host->card);
+	mmc_release_host(host);
+
+	/*
+	 * If the card looked to be the same as before suspending, then
+	 * we proceed to resume all card functions.  If one of them returns
+	 * an error then we simply return that error to the core and the
+	 * card will be redetected as new.  It is the responsibility of
+	 * the function driver to perform further tests with the extra
+	 * knowledge it has of the card to confirm the card is indeed the
+	 * same as before suspending (same MAC address for network cards,
+	 * etc.) and return an error otherwise.
+	 */
+	for (i = 0; !err && i < host->card->sdio_funcs; i++) {
+		struct sdio_func *func = host->card->sdio_func[i];
+		if (func && sdio_func_present(func) && func->dev.driver) {
+			const struct dev_pm_ops *pmops = func->dev.driver->pm;
+			err = pmops->resume(&func->dev);
+		}
+	}
+
+	return err;
+}
 
 static const struct mmc_bus_ops mmc_sdio_ops = {
 	.remove = mmc_sdio_remove,
 	.detect = mmc_sdio_detect,
+	.suspend = mmc_sdio_suspend,
+	.resume = mmc_sdio_resume,
 };
 
 
diff --git a/drivers/mmc/core/sdio_io.c b/drivers/mmc/core/sdio_io.c
index f61fc2d4cd0a..ff69a2feecd5 100644
--- a/drivers/mmc/core/sdio_io.c
+++ b/drivers/mmc/core/sdio_io.c
@@ -635,3 +635,52 @@ void sdio_f0_writeb(struct sdio_func *func, unsigned char b, unsigned int addr,
 		*err_ret = ret;
 }
 EXPORT_SYMBOL_GPL(sdio_f0_writeb);
+
+/**
+ *	sdio_get_host_pm_caps - get host power management capabilities
+ *	@func: SDIO function attached to host
+ *
+ *	Returns a capability bitmask corresponding to power management
+ *	features supported by the host controller that the card function
+ *	might rely upon during a system suspend.  The host doesn't need
+ *	to be claimed, nor the function active, for this information to be
+ *	obtained.
+ */
+mmc_pm_flag_t sdio_get_host_pm_caps(struct sdio_func *func)
+{
+	BUG_ON(!func);
+	BUG_ON(!func->card);
+
+	return func->card->host->pm_caps;
+}
+EXPORT_SYMBOL_GPL(sdio_get_host_pm_caps);
+
+/**
+ *	sdio_set_host_pm_flags - set wanted host power management capabilities
+ *	@func: SDIO function attached to host
+ *
+ *	Set a capability bitmask corresponding to wanted host controller
+ *	power management features for the upcoming suspend state.
+ *	This must be called, if needed, each time the suspend method of
+ *	the function driver is called, and must contain only bits that
+ *	were returned by sdio_get_host_pm_caps().
+ *	The host doesn't need to be claimed, nor the function active,
+ *	for this information to be set.
+ */
+int sdio_set_host_pm_flags(struct sdio_func *func, mmc_pm_flag_t flags)
+{
+	struct mmc_host *host;
+
+	BUG_ON(!func);
+	BUG_ON(!func->card);
+
+	host = func->card->host;
+
+	if (flags & ~host->pm_caps)
+		return -EINVAL;
+
+	/* function suspend methods are serialized, hence no lock needed */
+	host->pm_flags |= flags;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(sdio_set_host_pm_flags);
diff --git a/drivers/mmc/core/sdio_ops.c b/drivers/mmc/core/sdio_ops.c
index 208beeb23ed6..b6304fb8bf90 100644
--- a/drivers/mmc/core/sdio_ops.c
+++ b/drivers/mmc/core/sdio_ops.c
@@ -67,6 +67,61 @@ int mmc_send_io_op_cond(struct mmc_host *host, u32 ocr, u32 *rocr)
 	return err;
 }
 
+static int mmc_io_rw_direct_host(struct mmc_host *host, int write, unsigned fn,
+	unsigned addr, u8 in, u8 *out)
+{
+	struct mmc_command cmd;
+	int err;
+
+	BUG_ON(!host);
+	BUG_ON(fn > 7);
+
+	/* sanity check */
+	if (addr & ~0x1FFFF)
+		return -EINVAL;
+
+	memset(&cmd, 0, sizeof(struct mmc_command));
+
+	cmd.opcode = SD_IO_RW_DIRECT;
+	cmd.arg = write ? 0x80000000 : 0x00000000;
+	cmd.arg |= fn << 28;
+	cmd.arg |= (write && out) ? 0x08000000 : 0x00000000;
+	cmd.arg |= addr << 9;
+	cmd.arg |= in;
+	cmd.flags = MMC_RSP_SPI_R5 | MMC_RSP_R5 | MMC_CMD_AC;
+
+	err = mmc_wait_for_cmd(host, &cmd, 0);
+	if (err)
+		return err;
+
+	if (mmc_host_is_spi(host)) {
+		/* host driver already reported errors */
+	} else {
+		if (cmd.resp[0] & R5_ERROR)
+			return -EIO;
+		if (cmd.resp[0] & R5_FUNCTION_NUMBER)
+			return -EINVAL;
+		if (cmd.resp[0] & R5_OUT_OF_RANGE)
+			return -ERANGE;
+	}
+
+	if (out) {
+		if (mmc_host_is_spi(host))
+			*out = (cmd.resp[0] >> 8) & 0xFF;
+		else
+			*out = cmd.resp[0] & 0xFF;
+	}
+
+	return 0;
+}
+
+int mmc_io_rw_direct(struct mmc_card *card, int write, unsigned fn,
+	unsigned addr, u8 in, u8 *out)
+{
+	BUG_ON(!card);
+	return mmc_io_rw_direct_host(card->host, write, fn, addr, in, out);
+}
+
 int mmc_io_rw_extended(struct mmc_card *card, int write, unsigned fn,
 	unsigned addr, int incr_addr, u8 *buf, unsigned blocks, unsigned blksz)
 {
@@ -134,62 +189,7 @@ int mmc_io_rw_extended(struct mmc_card *card, int write, unsigned fn,
 	return 0;
 }
 
-static int mmc_io_rw_direct_host(struct mmc_host *host, int write, unsigned fn,
-	unsigned addr, u8 in, u8 *out)
-{
-	struct mmc_command cmd;
-	int err;
-
-	BUG_ON(!host);
-	BUG_ON(fn > 7);
-
-	/* sanity check */
-	if (addr & ~0x1FFFF)
-		return -EINVAL;
-
-	memset(&cmd, 0, sizeof(struct mmc_command));
-
-	cmd.opcode = SD_IO_RW_DIRECT;
-	cmd.arg = write ? 0x80000000 : 0x00000000;
-	cmd.arg |= fn << 28;
-	cmd.arg |= (write && out) ? 0x08000000 : 0x00000000;
-	cmd.arg |= addr << 9;
-	cmd.arg |= in;
-	cmd.flags = MMC_RSP_SPI_R5 | MMC_RSP_R5 | MMC_CMD_AC;
-
-	err = mmc_wait_for_cmd(host, &cmd, 0);
-	if (err)
-		return err;
-
-	if (mmc_host_is_spi(host)) {
-		/* host driver already reported errors */
-	} else {
-		if (cmd.resp[0] & R5_ERROR)
-			return -EIO;
-		if (cmd.resp[0] & R5_FUNCTION_NUMBER)
-			return -EINVAL;
-		if (cmd.resp[0] & R5_OUT_OF_RANGE)
-			return -ERANGE;
-	}
-
-	if (out) {
-		if (mmc_host_is_spi(host))
-			*out = (cmd.resp[0] >> 8) & 0xFF;
-		else
-			*out = cmd.resp[0] & 0xFF;
-	}
-
-	return 0;
-}
-
-int mmc_io_rw_direct(struct mmc_card *card, int write, unsigned fn,
-	unsigned addr, u8 in, u8 *out)
-{
-	BUG_ON(!card);
-	return mmc_io_rw_direct_host(card->host, write, fn, addr, in, out);
-}
-
-int sdio_go_idle(struct mmc_host *host)
+int sdio_reset(struct mmc_host *host)
 {
 	int ret;
 	u8 abort;
diff --git a/drivers/mmc/core/sdio_ops.h b/drivers/mmc/core/sdio_ops.h
index 9b546c71eb5e..85c7ecf809f4 100644
--- a/drivers/mmc/core/sdio_ops.h
+++ b/drivers/mmc/core/sdio_ops.h
@@ -18,6 +18,7 @@ int mmc_io_rw_direct(struct mmc_card *card, int write, unsigned fn,
 int mmc_io_rw_extended(struct mmc_card *card, int write, unsigned fn,
 	unsigned addr, int incr_addr, u8 *buf, unsigned blocks, unsigned blksz);
 int sdio_go_idle(struct mmc_host *host);
+int sdio_reset(struct mmc_host *host);
 
 #endif
 
diff --git a/drivers/mmc/host/mx_sdhci.c b/drivers/mmc/host/mx_sdhci.c
index 5a68a8e02fe2..732135308eb6 100644
--- a/drivers/mmc/host/mx_sdhci.c
+++ b/drivers/mmc/host/mx_sdhci.c
@@ -142,32 +142,32 @@ EXPORT_SYMBOL(mxc_mmc_force_detect);
 
 static void sdhci_dumpregs(struct sdhci_host *host)
 {
-	printk(KERN_DEBUG DRIVER_NAME
+	printk(KERN_INFO DRIVER_NAME
 	       ": ============== REGISTER DUMP ==============\n");
 
-	printk(KERN_DEBUG DRIVER_NAME ": Sys addr: 0x%08x | Version:  0x%08x\n",
+	printk(KERN_INFO DRIVER_NAME ": Sys addr: 0x%08x | Version:  0x%08x\n",
 	       readl(host->ioaddr + SDHCI_DMA_ADDRESS),
 	       readl(host->ioaddr + SDHCI_HOST_VERSION));
-	printk(KERN_DEBUG DRIVER_NAME ": Blk size: 0x%08x | Blk cnt:  0x%08x\n",
+	printk(KERN_INFO DRIVER_NAME ": Blk size: 0x%08x | Blk cnt:  0x%08x\n",
 	       (readl(host->ioaddr + SDHCI_BLOCK_SIZE) & 0xFFFF),
 	       (readl(host->ioaddr + SDHCI_BLOCK_COUNT) >> 16));
-	printk(KERN_DEBUG DRIVER_NAME ": Argument: 0x%08x | Trn mode: 0x%08x\n",
+	printk(KERN_INFO DRIVER_NAME ": Argument: 0x%08x | Trn mode: 0x%08x\n",
 	       readl(host->ioaddr + SDHCI_ARGUMENT),
 	       readl(host->ioaddr + SDHCI_TRANSFER_MODE));
-	printk(KERN_DEBUG DRIVER_NAME ": Present:  0x%08x | Host ctl: 0x%08x\n",
+	printk(KERN_INFO DRIVER_NAME ": Present:  0x%08x | Host ctl: 0x%08x\n",
 	       readl(host->ioaddr + SDHCI_PRESENT_STATE),
 	       readl(host->ioaddr + SDHCI_HOST_CONTROL));
-	printk(KERN_DEBUG DRIVER_NAME ": Clock:    0x%08x\n",
+	printk(KERN_INFO DRIVER_NAME ": Clock:    0x%08x\n",
 	       readl(host->ioaddr + SDHCI_CLOCK_CONTROL));
-	printk(KERN_DEBUG DRIVER_NAME ": Int stat: 0x%08x\n",
+	printk(KERN_INFO DRIVER_NAME ": Int stat: 0x%08x\n",
 	       readl(host->ioaddr + SDHCI_INT_STATUS));
-	printk(KERN_DEBUG DRIVER_NAME ": Int enab: 0x%08x | Sig enab: 0x%08x\n",
+	printk(KERN_INFO DRIVER_NAME ": Int enab: 0x%08x | Sig enab: 0x%08x\n",
 	       readl(host->ioaddr + SDHCI_INT_ENABLE),
 	       readl(host->ioaddr + SDHCI_SIGNAL_ENABLE));
-	printk(KERN_DEBUG DRIVER_NAME ": Caps:     0x%08x\n",
+	printk(KERN_INFO DRIVER_NAME ": Caps:     0x%08x\n",
 	       readl(host->ioaddr + SDHCI_CAPABILITIES));
 
-	printk(KERN_DEBUG DRIVER_NAME
+	printk(KERN_INFO DRIVER_NAME
 	       ": ===========================================\n");
 }
 
@@ -506,6 +506,30 @@ static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_data *data)
 		host->flags &= ~SDHCI_REQ_USE_DMA;
 	}
 
+	if (cpu_is_mx25() && (data->blksz * data->blocks < 0x10)) {
+		host->flags &= ~SDHCI_REQ_USE_DMA;
+		DBG("Reverting to PIO in small data transfer.\n");
+		writel(readl(host->ioaddr + SDHCI_INT_ENABLE)
+				| SDHCI_INT_DATA_AVAIL
+				| SDHCI_INT_SPACE_AVAIL,
+				host->ioaddr + SDHCI_INT_ENABLE);
+		writel(readl(host->ioaddr + SDHCI_SIGNAL_ENABLE)
+				| SDHCI_INT_DATA_AVAIL
+				| SDHCI_INT_SPACE_AVAIL,
+				host->ioaddr + SDHCI_SIGNAL_ENABLE);
+	} else if (cpu_is_mx25() && (host->flags & SDHCI_USE_DMA)) {
+		host->flags |= SDHCI_REQ_USE_DMA;
+		DBG("Reverting to DMA in large data transfer.\n");
+		writel(readl(host->ioaddr + SDHCI_INT_ENABLE)
+				& ~(SDHCI_INT_DATA_AVAIL
+				| SDHCI_INT_SPACE_AVAIL),
+				host->ioaddr + SDHCI_INT_ENABLE);
+		writel(readl(host->ioaddr + SDHCI_SIGNAL_ENABLE)
+				& ~(SDHCI_INT_DATA_AVAIL
+				| SDHCI_INT_SPACE_AVAIL),
+				host->ioaddr + SDHCI_SIGNAL_ENABLE);
+	}
+
 	if (host->flags & SDHCI_REQ_USE_DMA) {
 		int i;
 		struct scatterlist *tsg;
@@ -644,7 +668,7 @@ static void sdhci_send_command(struct sdhci_host *host, struct mmc_command *cmd)
 	WARN_ON(host->cmd);
 
 	/* Wait max 10 ms */
-	timeout = 5000;
+	timeout = 500;
 
 	mask = SDHCI_CMD_INHIBIT;
 	if ((cmd->data != NULL) || (cmd->flags & MMC_RSP_BUSY))
@@ -695,7 +719,7 @@ static void sdhci_send_command(struct sdhci_host *host, struct mmc_command *cmd)
 			mode |= SDHCI_TRNS_READ;
 		else
 			mode &= ~SDHCI_TRNS_READ;
-		if (host->flags & SDHCI_USE_DMA)
+		if (host->flags & SDHCI_REQ_USE_DMA)
 			mode |= SDHCI_TRNS_DMA;
 		if (host->flags & SDHCI_USE_EXTERNAL_DMA)
 			DBG("Prepare data completely in %s transfer mode.\n",
@@ -727,6 +751,11 @@ static void sdhci_send_command(struct sdhci_host *host, struct mmc_command *cmd)
 		flags |= SDHCI_CMD_DATA;
 
 	mode |= SDHCI_MAKE_CMD(cmd->opcode, flags);
+	if (host->mmc->ios.bus_width & MMC_BUS_WIDTH_DDR) {
+		/* Eanble the DDR mode */
+		mode |= SDHCI_TRNS_DDR_EN;
+	} else
+		mode &= ~SDHCI_TRNS_DDR_EN;
 	DBG("Complete sending cmd, transfer mode would be 0x%x.\n", mode);
 	writel(mode, host->ioaddr + SDHCI_TRANSFER_MODE);
 }
@@ -775,6 +804,7 @@ static void sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
 	int clk_rate = 0;
 	u32 clk;
 	unsigned long timeout;
+	struct mmc_ios ios = host->mmc->ios;
 
 	if (clock == 0) {
 		goto out;
@@ -784,17 +814,21 @@ static void sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
 			host->plat_data->clk_flg = 1;
 		}
 	}
-	if (clock == host->clock)
+
+	if (clock == host->clock && !(ios.bus_width & MMC_BUS_WIDTH_DDR))
 		return;
 
 	clk_rate = clk_get_rate(host->clk);
 	clk = readl(host->ioaddr + SDHCI_CLOCK_CONTROL) & ~SDHCI_CLOCK_MASK;
-	if (!cpu_is_mx53())
+	if (cpu_is_mx53() || cpu_is_mx50())
+		writel(clk | SDHCI_CLOCK_SDCLKFS1,
+				host->ioaddr + SDHCI_CLOCK_CONTROL);
+	else
 		writel(clk, host->ioaddr + SDHCI_CLOCK_CONTROL);
 
 	if (clock == host->min_clk)
 		prescaler = 16;
-	else if (cpu_is_mx53())
+	else if (cpu_is_mx53() || cpu_is_mx50())
 		prescaler = 1;
 	else
 		prescaler = 0;
@@ -820,6 +854,21 @@ static void sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
 	DBG("prescaler = 0x%x, divider = 0x%x\n", prescaler, div);
 	clk |= (prescaler << 8) | (div << 4);
 
+	if (host->plat_data->clk_always_on
+		| (host->mmc->card && mmc_card_sdio(host->mmc->card)))
+		clk |= SDHCI_CLOCK_PER_EN | SDHCI_CLOCK_HLK_EN
+			| SDHCI_CLOCK_IPG_EN;
+	else
+		clk &= ~(SDHCI_CLOCK_PER_EN | SDHCI_CLOCK_HLK_EN
+			| SDHCI_CLOCK_IPG_EN);
+
+	/* Configure the clock delay line */
+	if ((host->plat_data->vendor_ver >= ESDHC_VENDOR_V3)
+		&& host->plat_data->dll_override_en)
+		writel((host->plat_data->dll_delay_cells << 10)
+			| DLL_CTRL_SLV_OVERRIDE,
+			host->ioaddr + SDHCI_DLL_CONTROL);
+
 	/* Configure the clock control register */
 	clk |=
 	    (readl(host->ioaddr + SDHCI_CLOCK_CONTROL) & (~SDHCI_CLOCK_MASK));
@@ -830,7 +879,7 @@ static void sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
 		       host->ioaddr + SDHCI_CLOCK_CONTROL);
 
 	/* Wait max 10 ms */
-	timeout = 5000;
+	timeout = 500;
 	while (timeout > 0) {
 		timeout--;
 		udelay(20);
@@ -933,8 +982,8 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	u32 tmp;
 	mxc_dma_device_t dev_id = 0;
 
-	DBG("%s: clock %u, bus %lu, power %u, vdd %u\n", DRIVER_NAME,
-	    ios->clock, 1UL << ios->bus_width, ios->power_mode, ios->vdd);
+	DBG("%s: clock %u, bus %u, power %u, vdd %u\n", DRIVER_NAME,
+	    ios->clock, ios->bus_width, ios->power_mode, ios->vdd);
 
 	host = mmc_priv(mmc);
 
@@ -1000,10 +1049,10 @@ static void sdhci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	tmp = readl(host->ioaddr + SDHCI_HOST_CONTROL);
 
-	if (ios->bus_width == MMC_BUS_WIDTH_4) {
+	if ((ios->bus_width & ~MMC_BUS_WIDTH_DDR) == MMC_BUS_WIDTH_4) {
 		tmp &= ~SDHCI_CTRL_8BITBUS;
 		tmp |= SDHCI_CTRL_4BITBUS;
-	} else if (ios->bus_width == MMC_BUS_WIDTH_8) {
+	} else if ((ios->bus_width & ~MMC_BUS_WIDTH_DDR) == MMC_BUS_WIDTH_8) {
 		tmp &= ~SDHCI_CTRL_4BITBUS;
 		tmp |= SDHCI_CTRL_8BITBUS;
 	} else if (ios->bus_width == MMC_BUS_WIDTH_1) {
@@ -1044,7 +1093,7 @@ static void sdhci_enable_sdio_irq(struct mmc_host *mmc, int enable)
 {
 	struct sdhci_host *host;
 	unsigned long flags;
-	u32 ier, prot, clk, present;
+	u32 ier, prot, present;
 
 	host = mmc_priv(mmc);
 
@@ -1057,19 +1106,12 @@ static void sdhci_enable_sdio_irq(struct mmc_host *mmc, int enable)
 		if (--(host->sdio_enable))
 			goto exit_unlock;
 	}
-	/* Enable the clock */
-	if (!host->plat_data->clk_flg) {
-		clk_enable(host->clk);
-		host->plat_data->clk_flg = 1;
-	}
-	ier = readl(host->ioaddr + SDHCI_SIGNAL_ENABLE);
+
+	ier = readl(host->ioaddr + SDHCI_INT_ENABLE);
 	prot = readl(host->ioaddr + SDHCI_HOST_CONTROL);
-	clk = readl(host->ioaddr + SDHCI_CLOCK_CONTROL);
 
 	if (enable) {
 		ier |= SDHCI_INT_CARD_INT;
-		prot |= SDHCI_CTRL_D3CD;
-		clk |= SDHCI_CLOCK_PER_EN | SDHCI_CLOCK_IPG_EN;
 		present = readl(host->ioaddr + SDHCI_PRESENT_STATE);
 		if ((present & SDHCI_CARD_INT_MASK) != SDHCI_CARD_INT_ID)
 			writel(SDHCI_INT_CARD_INT,
@@ -1077,12 +1119,24 @@ static void sdhci_enable_sdio_irq(struct mmc_host *mmc, int enable)
 	} else {
 		ier &= ~SDHCI_INT_CARD_INT;
 		prot &= ~SDHCI_CTRL_D3CD;
-		clk &= ~(SDHCI_CLOCK_PER_EN | SDHCI_CLOCK_IPG_EN);
 	}
 
 	writel(prot, host->ioaddr + SDHCI_HOST_CONTROL);
+	writel(ier, host->ioaddr + SDHCI_INT_ENABLE);
 	writel(ier, host->ioaddr + SDHCI_SIGNAL_ENABLE);
-	writel(clk, host->ioaddr + SDHCI_CLOCK_CONTROL);
+
+	/*
+	 * Using D3CD to manually driver the HW to re-sample the SDIO interrupt
+	 * on bus one more time to guarantee the SDIO interrupt signal sent
+	 * from card during the interrupt signal disabled period will not
+	 * be lost.
+	 */
+	prot |= SDHCI_CTRL_CDSS;
+	writel(prot, host->ioaddr + SDHCI_HOST_CONTROL);
+	prot &= ~SDHCI_CTRL_D3CD;
+	writel(prot, host->ioaddr + SDHCI_HOST_CONTROL);
+	prot |= SDHCI_CTRL_D3CD;
+	writel(prot, host->ioaddr + SDHCI_HOST_CONTROL);
 
 	mmiowb();
       exit_unlock:
@@ -1208,7 +1262,7 @@ static void sdhci_tasklet_finish(unsigned long param)
 	 * The root cause is that the ROM code don't ensure
 	 * the SD/MMC clk is running when boot system.
 	 * */
-	if (!machine_is_mx35_3ds() && req_done && host->plat_data->clk_flg &&
+	if (req_done && host->plat_data->clk_flg &&
 	    !(host->mmc && host->mmc->card && mmc_card_sdio(host->mmc->card))) {
 		clk_disable(host->clk);
 		host->plat_data->clk_flg = 0;
@@ -1786,8 +1840,10 @@ static int __devinit sdhci_probe_slot(struct platform_device
 
 	/* Get the SDHC clock from clock system APIs */
 	host->clk = clk_get(&pdev->dev, mmc_plat->clock_mmc);
-	if (NULL == host->clk)
+	if (NULL == host->clk) {
 		printk(KERN_ERR "MXC MMC can't get clock.\n");
+		goto out1;
+	}
 	DBG("SDHC:%d clock:%lu\n", pdev->id, clk_get_rate(host->clk));
 
 	host->res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -2001,9 +2057,6 @@ static int __devinit sdhci_probe_slot(struct platform_device
 		}
 		mxc_dma_callback_set(host->dma, sdhci_dma_irq, (void *)host);
 	}
-#ifdef CONFIG_MMC_DEBUG
-	sdhci_dumpregs(host);
-#endif
 
 	mmiowb();
 
diff --git a/drivers/mmc/host/mx_sdhci.h b/drivers/mmc/host/mx_sdhci.h
index 0bd79934952e..83d02975ecd1 100644
--- a/drivers/mmc/host/mx_sdhci.h
+++ b/drivers/mmc/host/mx_sdhci.h
@@ -2,7 +2,6 @@
  *  linux/drivers/mmc/host/mx_sdhci.h - Secure Digital Host
  *  Controller Interface driver
  *
- *  Copyright (C) 2005-2007 Pierre Ossman, All Rights Reserved.
  *  Copyright (C) 2008-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -28,6 +27,7 @@
 #define  SDHCI_TRNS_DMA		0x00000001
 #define  SDHCI_TRNS_BLK_CNT_EN	0x00000002
 #define  SDHCI_TRNS_ACMD12	0x00000004
+#define  SDHCI_TRNS_DDR_EN 	0x00000008
 #define  SDHCI_TRNS_READ	0x00000010
 #define  SDHCI_TRNS_MULTI	0x00000020
 #define  SDHCI_TRNS_DPSEL	0x00200000
@@ -69,6 +69,7 @@
 #define  SDHCI_CTRL_4BITBUS	0x00000002
 #define  SDHCI_CTRL_8BITBUS	0x00000004
 #define  SDHCI_CTRL_HISPD	0x00000004
+#define  SDHCI_CTRL_CDSS       0x80
 #define  SDHCI_CTRL_DMA_MASK	0x18
 #define   SDHCI_CTRL_SDMA	0x00
 #define   SDHCI_CTRL_ADMA1	0x08
@@ -95,6 +96,7 @@
 #define  SDHCI_CLOCK_PER_EN	0x00000004
 #define  SDHCI_CLOCK_HLK_EN	0x00000002
 #define  SDHCI_CLOCK_IPG_EN	0x00000001
+#define  SDHCI_CLOCK_SDCLKFS1 	0x00000100
 #define  SDHCI_CLOCK_MASK 	0x0000FFFF
 
 #define SDHCI_TIMEOUT_CONTROL	0x2E
@@ -188,6 +190,18 @@
 #define SDHCI_ADMA_ADDRESS	0x58
 
 /* 60-FB reserved */
+#define SDHCI_DLL_CONTROL 	0x60
+#define DLL_CTRL_ENABLE 	0x00000001
+#define DLL_CTRL_RESET 		0x00000002
+#define DLL_CTRL_SLV_FORCE_UPD 	0x00000004
+#define DLL_CTRL_SLV_OVERRIDE	0x00000200
+#define DLL_CTRL_SLV_DLY_TAR 	0x00000000
+#define DLL_CTRL_SLV_UP_INT 	0x00200000
+#define DLL_CTRL_REF_UP_INT 	0x20000000
+
+#define SDHCI_DLL_STATUS 	0x64
+#define DLL_STS_SLV_LOCK 	0x00000001
+#define DLL_STS_REF_LOCK 	0x00000002
 
 /* ADMA Addr Descriptor Attribute Filed */
 enum {
@@ -207,6 +221,7 @@ enum {
 #define   SDHCI_SPEC_100	0
 #define   SDHCI_SPEC_200	1
 #define   ESDHC_VENDOR_V22 	0x12
+#define   ESDHC_VENDOR_V3 	0x13
 
 struct sdhci_chip;
 
diff --git a/drivers/mmc/host/mxc_mmc.c b/drivers/mmc/host/mxc_mmc.c
index 980dc98c9b5f..9cb492f40145 100644
--- a/drivers/mmc/host/mxc_mmc.c
+++ b/drivers/mmc/host/mxc_mmc.c
@@ -13,7 +13,7 @@
  */
 
 /*
- * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -1232,7 +1232,7 @@ static int mxcmci_probe(struct platform_device *pdev)
 	mmc->f_max = mmc_plat->max_clk;
 	mmc->max_req_size = 32 * 1024;
 	mmc->max_seg_size = mmc->max_req_size;
-	mmc->max_blk_count = 65536;
+	mmc->max_blk_count = 32;
 
 	spin_lock_init(&host->lock);
 	host->mmc = mmc;
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index c60352247c49..b7210b7d7af3 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -49,7 +49,7 @@
 #define MXS_MMC_DETECT_TIMEOUT		(HZ/2)
 
 /* Max value supported for XFER_COUNT */
-#define SSP_BUFFER_SIZE		(65536)
+#define SSP_BUFFER_SIZE		(65535)
 
 #ifndef BF
 #define BF(value, field) (((value) << BP_##field) & BM_##field)
@@ -93,6 +93,9 @@
 #define BF_SSP_BLOCK_SIZE_BLOCK_SIZE(v)  \
 		(((v) << 16) & BM_SSP_BLOCK_SIZE_BLOCK_SIZE)
 #endif
+#ifndef BM_SSP_CMD0_DBL_DATA_RATE_EN
+#define BM_SSP_CMD0_DBL_DATA_RATE_EN	0x02000000
+#endif
 
 struct mxs_mmc_host {
 	struct device *dev;
@@ -159,6 +162,7 @@ static inline int mxs_mmc_is_plugged(struct mxs_mmc_host *host)
 	return !(status & BM_SSP_STATUS_CARD_DETECT);
 }
 
+static void mxs_mmc_reset(struct mxs_mmc_host *host);
 /* Card detection polling function */
 static void mxs_mmc_detect_poll(unsigned long arg)
 {
@@ -167,6 +171,8 @@ static void mxs_mmc_detect_poll(unsigned long arg)
 
 	card_status = mxs_mmc_is_plugged(host);
 	if (card_status != host->present) {
+		/* Reset MMC block */
+		mxs_mmc_reset(host);
 		host->present = card_status;
 		mmc_detect_change(host->mmc, 0);
 	}
@@ -183,6 +189,12 @@ static void mxs_mmc_detect_poll(unsigned long arg)
 				BM_SSP_CTRL1_RECV_TIMEOUT_IRQ   | \
 				BM_SSP_CTRL1_FIFO_OVERRUN_IRQ)
 
+#define MXS_MMC_ERR_BITS (BM_SSP_CTRL1_RESP_ERR_IRQ       | \
+				BM_SSP_CTRL1_RESP_TIMEOUT_IRQ   | \
+				BM_SSP_CTRL1_DATA_TIMEOUT_IRQ   | \
+				BM_SSP_CTRL1_DATA_CRC_IRQ       | \
+				BM_SSP_CTRL1_RECV_TIMEOUT_IRQ)
+
 /* SSP DMA interrupt handler */
 static irqreturn_t mmc_irq_handler(int irq, void *dev_id)
 {
@@ -198,19 +210,17 @@ static irqreturn_t mmc_irq_handler(int irq, void *dev_id)
 		/* STOP the dma transfer here. */
 		mxs_dma_cooked(host->dmach, NULL);
 	}
-	host->status =
-	    __raw_readl(host->ssp_base + HW_SSP_STATUS);
 
-	if (host->cmd)		/* else it is a bogus interrupt */
-		complete(&host->dma_done);
+	if ((irq == host->dmairq) || (c1 & MXS_MMC_ERR_BITS))
+		if (host->cmd) {
+			host->status =
+				__raw_readl(host->ssp_base + HW_SSP_STATUS);
+			complete(&host->dma_done);
+		}
 
-	if ((c1 & BM_SSP_CTRL1_SDIO_IRQ) && (c1 & BM_SSP_CTRL1_SDIO_IRQ_EN)) {
-		__raw_writel(BM_SSP_CTRL0_SDIO_IRQ_CHECK, host->ssp_base + \
-			HW_SSP_CTRL0_CLR);
-		__raw_writel(BM_SSP_CTRL1_SDIO_IRQ_EN, host->ssp_base + \
-			HW_SSP_CTRL1_CLR);
+	if ((c1 & BM_SSP_CTRL1_SDIO_IRQ) && (c1 & BM_SSP_CTRL1_SDIO_IRQ_EN))
 		mmc_signal_sdio_irq(host->mmc);
-	}
+
 	return IRQ_HANDLED;
 }
 
@@ -239,6 +249,7 @@ static void mxs_mmc_bc(struct mxs_mmc_host *host)
 {
 	struct mmc_command *cmd = host->cmd;
 	struct mxs_dma_desc *dma_desc = host->dma_desc;
+	unsigned long flags;
 
 	dma_desc->cmd.cmd.bits.command = NO_DMA_XFER;
 	dma_desc->cmd.cmd.bits.irq = 1;
@@ -255,7 +266,8 @@ static void mxs_mmc_bc(struct mxs_mmc_host *host)
 
 	if (host->sdio_irq_en) {
 		dma_desc->cmd.pio_words[0] |= BM_SSP_CTRL0_SDIO_IRQ_CHECK;
-		dma_desc->cmd.pio_words[1] |= BM_SSP_CMD0_CONT_CLKING_EN;
+		dma_desc->cmd.pio_words[1] |= BM_SSP_CMD0_CONT_CLKING_EN \
+			| BM_SSP_CMD0_SLOW_CLKING_EN;
 	}
 
 	init_completion(&host->dma_done);
@@ -265,6 +277,7 @@ static void mxs_mmc_bc(struct mxs_mmc_host *host)
 	dev_dbg(host->dev, "%s start DMA.\n", __func__);
 	if (mxs_dma_enable(host->dmach) < 0)
 		dev_err(host->dev, "mmc_dma_enable failed\n");
+
 	wait_for_completion(&host->dma_done);
 
 	cmd->error = mxs_mmc_cmd_error(host->status);
@@ -273,6 +286,7 @@ static void mxs_mmc_bc(struct mxs_mmc_host *host)
 		dev_dbg(host->dev, "Command error 0x%x\n", cmd->error);
 		mxs_dma_reset(host->dmach);
 	}
+	mxs_dma_disable(host->dmach);
 }
 
 /* Send the ac command to the device */
@@ -284,6 +298,7 @@ static void mxs_mmc_ac(struct mxs_mmc_host *host)
 	u32 ssp_ctrl0;
 	u32 ssp_cmd0;
 	u32 ssp_cmd1;
+	unsigned long flags;
 
 	ignore_crc = (mmc_resp_type(cmd) & MMC_RSP_CRC) ?
 	    0 : BM_SSP_CTRL0_IGNORE_CRC;
@@ -305,7 +320,8 @@ static void mxs_mmc_ac(struct mxs_mmc_host *host)
 
 	if (host->sdio_irq_en) {
 		ssp_ctrl0 |= BM_SSP_CTRL0_SDIO_IRQ_CHECK;
-		ssp_cmd0 |= BM_SSP_CMD0_CONT_CLKING_EN;
+		ssp_cmd0 |= BM_SSP_CMD0_CONT_CLKING_EN \
+			| BM_SSP_CMD0_SLOW_CLKING_EN;
 	}
 
 	dma_desc->cmd.pio_words[0] = ssp_ctrl0;
@@ -356,6 +372,7 @@ static void mxs_mmc_ac(struct mxs_mmc_host *host)
 		dev_dbg(host->dev, "Command error 0x%x\n", cmd->error);
 		mxs_dma_reset(host->dmach);
 	}
+	mxs_dma_disable(host->dmach);
 }
 
 /* Copy data between sg list and dma buffer */
@@ -451,6 +468,7 @@ static void mxs_mmc_adtc(struct mxs_mmc_host *host)
 
 	u32 data_size = cmd->data->blksz * cmd->data->blocks;
 	u32 log2_block_size;
+	unsigned long flags;
 
 	ignore_crc = mmc_resp_type(cmd) & MMC_RSP_CRC ? 0 : 1;
 	resp = mmc_resp_type(cmd) & MMC_RSP_PRESENT ? 1 : 0;
@@ -540,6 +558,9 @@ static void mxs_mmc_adtc(struct mxs_mmc_host *host)
 	dev_dbg(host->dev, "%s blksz is 0x%x.\n", __func__, log2_block_size);
 
 	if (ssp_ver_major > 3) {
+		/* Configure the CMD0 */
+		ssp_cmd0 = BF(cmd->opcode, SSP_CMD0_CMD);
+
 		/* Configure the BLOCK SIZE and BLOCK COUNT */
 		if ((1<<log2_block_size) != cmd->data->blksz) {
 			BUG_ON(cmd->data->blocks > 1);
@@ -548,20 +569,32 @@ static void mxs_mmc_adtc(struct mxs_mmc_host *host)
 			val = BF(log2_block_size, SSP_BLOCK_SIZE_BLOCK_SIZE) |
 			BF(cmd->data->blocks - 1, SSP_BLOCK_SIZE_BLOCK_COUNT);
 			__raw_writel(val, host->ssp_base + HW_SSP_BLOCK_SIZE);
-		}
-		/* Configure the CMD0 */
-		ssp_cmd0 = BF(cmd->opcode, SSP_CMD0_CMD);
-	} else
-		ssp_cmd0 =
-		    BF(log2_block_size, SSP_BLOCK_SIZE_BLOCK_SIZE) |
-		    BF(cmd->opcode, SSP_CMD0_CMD) |
-		    BF(cmd->data->blocks - 1, SSP_BLOCK_SIZE_BLOCK_COUNT);
+			if (host->mmc->ios.bus_width & MMC_BUS_WIDTH_DDR)
+				/* Enable the DDR mode */
+				ssp_cmd0 |= BM_SSP_CMD0_DBL_DATA_RATE_EN;
+			else
+				ssp_cmd0 &= ~BM_SSP_CMD0_DBL_DATA_RATE_EN;
 
+		}
+	} else {
+		if ((1<<log2_block_size) != cmd->data->blksz) {
+			BUG_ON(cmd->data->blocks > 1);
+			ssp_cmd0 =
+				BF(0, SSP_BLOCK_SIZE_BLOCK_SIZE) |
+				BF(cmd->opcode, SSP_CMD0_CMD) |
+				BF(0, SSP_BLOCK_SIZE_BLOCK_COUNT);
+		} else
+			ssp_cmd0 =
+			BF(log2_block_size, SSP_BLOCK_SIZE_BLOCK_SIZE) |
+			BF(cmd->opcode, SSP_CMD0_CMD) |
+			BF(cmd->data->blocks - 1, SSP_BLOCK_SIZE_BLOCK_COUNT);
+	}
 	if (host->sdio_irq_en) {
 		ssp_ctrl0 |= BM_SSP_CTRL0_SDIO_IRQ_CHECK;
-		ssp_cmd0 |= BM_SSP_CMD0_CONT_CLKING_EN;
+		ssp_cmd0 |= BM_SSP_CMD0_CONT_CLKING_EN \
+			| BM_SSP_CMD0_SLOW_CLKING_EN;
 	}
-	if (cmd->opcode == 12)
+	if ((cmd->opcode == 12) || (cmd->opcode == 53))
 		ssp_cmd0 |= BM_SSP_CMD0_APPEND_8CYC;
 
 	ssp_cmd1 = BF(cmd->arg, SSP_CMD1_CMD_ARG);
@@ -628,6 +661,7 @@ static void mxs_mmc_adtc(struct mxs_mmc_host *host)
 		dev_dbg(host->dev, "Transferred %u bytes\n",
 			cmd->data->bytes_xfered);
 	}
+	mxs_dma_disable(host->dmach);
 }
 
 /* Begin sedning a command to the card */
@@ -672,6 +706,13 @@ static void mxs_mmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
 
 	dev_dbg(host->dev, "MMC request\n");
 
+	if (!host->present) {
+		mrq->cmd->error = -ETIMEDOUT;
+		mmc_request_done(mmc, mrq);
+		return;
+	}
+
+	BUG_ON(host->mrq != NULL);
 	host->mrq = mrq;
 
 	mxs_mmc_start_cmd(host, mrq->cmd);
@@ -777,9 +818,9 @@ static void mxs_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		dev_warn(host->dev,
 			 "Platform does not support CMD pin pullup control\n");
 
-	if (ios->bus_width == MMC_BUS_WIDTH_8)
+	if ((ios->bus_width & ~MMC_BUS_WIDTH_DDR) == MMC_BUS_WIDTH_8)
 		host->bus_width = 2;
-	else if (ios->bus_width == MMC_BUS_WIDTH_4)
+	else if ((ios->bus_width & ~MMC_BUS_WIDTH_DDR) == MMC_BUS_WIDTH_4)
 		host->bus_width = 1;
 	else
 		host->bus_width = 0;
@@ -851,7 +892,6 @@ static void mxs_mmc_reset(struct mxs_mmc_host *host)
 	/* Configure SSP Control Register 1 */
 	ssp_ctrl1 =
 	    BM_SSP_CTRL1_DMA_ENABLE |
-	    BM_SSP_CTRL1_POLARITY |
 	    BM_SSP_CTRL1_RECV_TIMEOUT_IRQ_EN |
 	    BM_SSP_CTRL1_DATA_CRC_IRQ_EN |
 	    BM_SSP_CTRL1_DATA_TIMEOUT_IRQ_EN |
diff --git a/drivers/mtd/devices/mxc_dataflash.c b/drivers/mtd/devices/mxc_dataflash.c
index ab75d743a05b..0ed701d6778c 100644
--- a/drivers/mtd/devices/mxc_dataflash.c
+++ b/drivers/mtd/devices/mxc_dataflash.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  * (c) 2005 MontaVista Software, Inc.
  *
  * This code is based on mtd_dataflash.c by adding FSL spi access.
@@ -22,10 +22,10 @@
 #include <linux/err.h>
 
 #include <linux/spi/spi.h>
-#include <linux/spi/flash.h>
 
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
+#include <asm/mach/flash.h>
 
 /*
  * DataFlash is a kind of SPI flash.  Most AT45 chips have two buffers in
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 8f1eebf8d3b3..1a85f6d5543b 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -434,49 +434,9 @@ config MXC_NAND_LOW_LEVEL_ERASE
 	  This enables the erase of whole NAND flash. By
 	  default low level erase operation is disabled.
 
-config MTD_NAND_GPMI_LBA
-	tristate "GPMI LBA NAND driver"
-	depends on MTD_NAND && ARCH_STMP3XXX
-	help
-	 Enables support of LBA devices on GPMI on 37xx/378x SigmaTel
-	 boards
-
-config MTD_NAND_GPMI
-	tristate "GPMI NAND driver"
-	depends on MTD_NAND && ARCH_STMP3XXX && !MTD_NAND_GPMI_LBA
-	help
-	 Enables support of NAND devices on GPMI on 37xx/378x SigmaTel
-	 boards
-
-config MTD_NAND_GPMI_SYSFS_ENTRIES
-	bool "Create /sys entries for GPMI device"
-	depends on MTD_NAND_GPMI
-	help
-	  Check this to enable /sys entries for GPMI devices
-
-config MTD_NAND_GPMI_BCH
-	bool "Enable BCH HWECC"
-	depends on MTD_NAND_GPMI
-	depends on ARCH_STMP378X
-	default y
-	help
-	  Check this to enable /sys entries for GPMI devices
-
-config MTD_NAND_GPMI_TA1
-	bool "Support for TA1 NCB format (Hamming code 22,16)"
-	depends on MTD_NAND_GPMI
-	depends on ARCH_STMP378X
-	default y
-
-config MTD_NAND_GPMI_TA3
-	bool "Support for TA3 NCB format (Hamming code 13,8)"
-	depends on MTD_NAND_GPMI
-	depends on ARCH_STMP378X
-	default y
-
-config MTD_NAND_GPMI1
-        tristate "GPMI NAND Flash driver"
-        depends on MTD_NAND && ARCH_MX28
+config MTD_NAND_GPMI_NFC
+        tristate "GPMI NAND Flash Controller driver"
+        depends on MTD_NAND && (ARCH_MX23 || ARCH_MX28 || ARCH_MX50)
         help
          Enables NAND Flash support.
 
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index 730f5db16e1d..2245a8df441b 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -41,9 +41,7 @@ obj-$(CONFIG_MTD_NAND_IMX_NFC)		+= imx_nfc.o
 obj-$(CONFIG_MTD_NAND_MXC)		+= mxc_nand.o
 obj-$(CONFIG_MTD_NAND_MXC_V2)		+= mxc_nd2.o nand_device_info.o
 obj-$(CONFIG_MTD_NAND_MXC_V3)		+= mxc_nd2.o nand_device_info.o
-obj-$(CONFIG_MTD_NAND_GPMI)		+= gpmi/ nand_device_info.o
-obj-$(CONFIG_MTD_NAND_GPMI1)		+= gpmi1/ nand_device_info.o
-obj-$(CONFIG_MTD_NAND_GPMI_LBA)		+= lba/
+obj-$(CONFIG_MTD_NAND_GPMI_NFC)		+= gpmi-nfc/ nand_device_info.o
 obj-$(CONFIG_MTD_NAND_SOCRATES)		+= socrates_nand.o
 obj-$(CONFIG_MTD_NAND_TXX9NDFMC)	+= txx9ndfmc.o
 
diff --git a/drivers/mtd/nand/mxc_nd2.c b/drivers/mtd/nand/mxc_nd2.c
index 46e6380fe462..80533ac42e9c 100644
--- a/drivers/mtd/nand/mxc_nd2.c
+++ b/drivers/mtd/nand/mxc_nd2.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -32,12 +32,15 @@
 
 /* Global address Variables */
 static void __iomem *nfc_axi_base, *nfc_ip_base;
+static int nfc_irq;
 
 struct mxc_mtd_s {
 	struct mtd_info mtd;
 	struct nand_chip nand;
 	struct mtd_partition *parts;
 	struct device *dev;
+	int disable_bi_swap; /* disable bi swap */
+	int clk_active;
 };
 
 static struct mxc_mtd_s *mxc_nand_data;
@@ -117,6 +120,49 @@ static const char *part_probes[] = {
 
 static wait_queue_head_t irq_waitq;
 
+#if 0
+static void nand_page_dump(struct mtd_info *mtd, u8 *dbuf, u8* obuf)
+{
+        int i;
+
+        if (dbuf != NULL) {
+                printk("\nData buffer:");
+                for (i = 0; i < mtd->writesize; i++) {
+                        if (!(i % 8))   printk("\n%03x: ", i);
+                        printk("%02x ", dbuf[i]);
+                }
+        }
+        printk("\n");
+        if (obuf != NULL) {
+                printk("\nOOB buffer:");
+                for (i = 0; i < mtd->oobsize; i++) {
+                        if (!(i % 8))   printk("\n%02x: ", i);
+                        printk("%02x ", obuf[i]);
+                }
+        }
+        printk("\n");
+}
+#endif
+
+#ifdef CONFIG_MXC_NAND_SWAP_BI
+#define PART_UBOOT_SIZE         0xc0000
+#define SKIP_SWAP_BI_MAX_PAGE           (PART_UBOOT_SIZE / 0x800)
+inline int skip_swap_bi(int page)
+{
+        /**
+         * Seems that the boot code of the i.mx515 rom is not able to
+         * boot from a nand flash when the data has been written swapping
+	 * the bad block byte. Avoid doing that (the swapping) when
+	 * programming U-Boot into the flash.
+         */
+        if (page < SKIP_SWAP_BI_MAX_PAGE)
+                return 1;
+        return 0;
+}
+#else
+inline int skip_swap_bi(int page_addr) { return 1; }
+#endif
+
 static irqreturn_t mxc_nfc_irq(int irq, void *dev_id)
 {
 	/* Disable Interuupt */
@@ -126,6 +172,30 @@ static irqreturn_t mxc_nfc_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+static void mxc_nand_bi_swap(struct mtd_info *mtd, int page_addr)
+{
+	u16 ma, sa, nma, nsa;
+
+	if (!IS_LARGE_PAGE_NAND)
+		return;
+
+	/* Disable bi swap if the user set disable_bi_swap at sys entry */
+	if (mxc_nand_data->disable_bi_swap)
+		return;
+
+        if (skip_swap_bi(page_addr))
+                return;
+
+	ma = __raw_readw(BAD_BLK_MARKER_MAIN);
+	sa = __raw_readw(BAD_BLK_MARKER_SP);
+
+	nma = (ma & 0xFF00) | (sa >> 8);
+	nsa = (sa & 0x00FF) | (ma << 8);
+
+	__raw_writew(nma, BAD_BLK_MARKER_MAIN);
+	__raw_writew(nsa, BAD_BLK_MARKER_SP);
+}
+
 static void nfc_memcpy(void *dest, void *src, int len)
 {
 	u8 *d = dest;
@@ -287,6 +357,7 @@ static void auto_cmd_interleave(struct mtd_info *mtd, u16 cmd)
 			/* data transfer */
 			memcpy(MAIN_AREA0, dbuf, dlen);
 			copy_spare(mtd, obuf, SPARE_AREA0, olen, false);
+                        mxc_nand_bi_swap(mtd, page_addr - 1);
 
 			/* update the value */
 			dbuf += dlen;
@@ -316,6 +387,7 @@ static void auto_cmd_interleave(struct mtd_info *mtd, u16 cmd)
 			mxc_check_ecc_status(mtd);
 
 			/* data transfer */
+                        mxc_nand_bi_swap(mtd, page_addr - 1);
 			memcpy(dbuf, MAIN_AREA0, dlen);
 			copy_spare(mtd, obuf, SPARE_AREA0, olen, true);
 
@@ -558,10 +630,7 @@ static int mxc_check_ecc_status(struct mtd_info *mtd)
 	u32 ecc_stat, err;
 	int no_subpages = 1;
 	int ret = 0;
-	u8 ecc_bit_mask, err_limit;
-
-	ecc_bit_mask = (IS_4BIT_ECC ? 0x7 : 0xf);
-	err_limit = (IS_4BIT_ECC ? 0x4 : 0x8);
+	u8 ecc_bit_mask = 0xf;
 
 	no_subpages = mtd->writesize >> 9;
 
@@ -570,7 +639,7 @@ static int mxc_check_ecc_status(struct mtd_info *mtd)
 	ecc_stat = GET_NFC_ECC_STATUS();
 	do {
 		err = ecc_stat & ecc_bit_mask;
-		if (err > err_limit) {
+		if (err == ecc_bit_mask) {
 			mtd->ecc_stats.failed++;
 			printk(KERN_WARNING "UnCorrectable RS-ECC Error\n");
 			return -1;
@@ -580,8 +649,7 @@ static int mxc_check_ecc_status(struct mtd_info *mtd)
 		ecc_stat >>= 4;
 	} while (--no_subpages);
 
-	mtd->ecc_stats.corrected += ret;
-	pr_debug("%d Symbol Correctable RS-ECC Error\n", ret);
+	pr_debug("Correctable ECC Error(%d)\n", ret);
 
 	return ret;
 }
@@ -774,6 +842,30 @@ static int mxc_nand_verify_buf(struct mtd_info *mtd, const u_char * buf,
 }
 
 /*!
+ * This function will enable NFC clock
+ *
+ */
+static inline void mxc_nand_clk_enable(void)
+{
+	if (!mxc_nand_data->clk_active) {
+		clk_enable(nfc_clk);
+		mxc_nand_data->clk_active = 1;
+	}
+}
+
+/*!
+ * This function will disable NFC clock
+ *
+ */
+static inline void mxc_nand_clk_disable(void)
+{
+	if (mxc_nand_data->clk_active) {
+		clk_disable(nfc_clk);
+		mxc_nand_data->clk_active = 0;
+	}
+}
+
+/*!
  * This function is used by upper layer for select and deselect of the NAND
  * chip
  *
@@ -786,13 +878,14 @@ static void mxc_nand_select_chip(struct mtd_info *mtd, int chip)
 	switch (chip) {
 	case -1:
 		/* Disable the NFC clock */
-		clk_disable(nfc_clk);
+		mxc_nand_clk_disable();
+
 		break;
-	case 0 ... 7:
+	case 0 ... NFC_GET_MAXCHIP_SP():
 		/* Enable the NFC clock */
-		clk_enable(nfc_clk);
-
+		mxc_nand_clk_enable();
 		NFC_SET_NFC_ACTIVE_CS(chip);
+
 		break;
 
 	default:
@@ -930,6 +1023,7 @@ static void mxc_nand_command(struct mtd_info *mtd, unsigned command,
 		 */
 		nfc_memcpy(MAIN_AREA0, data_buf, mtd->writesize);
 		copy_spare(mtd, oob_buf, SPARE_AREA0, mtd->oobsize, false);
+                mxc_nand_bi_swap(mtd, page_addr);
 #endif
 
 		if (IS_LARGE_PAGE_NAND)
@@ -980,10 +1074,10 @@ static void mxc_nand_command(struct mtd_info *mtd, unsigned command,
 		 * byte alignment, so we can use
 		 * memcpy safely
 		 */
+                mxc_nand_bi_swap(mtd, page_addr);
 		nfc_memcpy(data_buf, MAIN_AREA0, mtd->writesize);
 		copy_spare(mtd, oob_buf, SPARE_AREA0, mtd->oobsize, true);
 #endif
-
 		break;
 
 	case NAND_CMD_READID:
@@ -1096,6 +1190,14 @@ static int mxc_nand_scan_bbt(struct mtd_info *mtd)
 	/* jffs2 not write oob */
 	mtd->flags &= ~MTD_OOB_WRITEABLE;
 
+	/* fix up the offset */
+	largepage_memorybased.offs = BAD_BLK_MARKER_OOB_OFFS;
+	/* keep compatible for bbt table with old soc */
+	if (cpu_is_mx53()) {
+		bbt_mirror_descr.offs = BAD_BLK_MARKER_OOB_OFFS + 2;
+		bbt_main_descr.offs = BAD_BLK_MARKER_OOB_OFFS + 2;
+	}
+
 	/* use flash based bbt */
 	this->bbt_td = &bbt_main_descr;
 	this->bbt_md = &bbt_mirror_descr;
@@ -1126,6 +1228,53 @@ static int mxc_nand_scan_bbt(struct mtd_info *mtd)
 	return nand_scan_bbt(mtd, this->badblock_pattern);
 }
 
+static int mxc_get_resources(struct platform_device *pdev)
+{
+	struct resource *r;
+	int error = 0;
+
+#define	MXC_NFC_NO_IP_REG \
+	(cpu_is_mx25() || cpu_is_mx31() || cpu_is_mx32() || cpu_is_mx35())
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		error = -ENXIO;
+		goto out_0;
+	}
+	nfc_axi_base = ioremap(r->start, resource_size(r));
+
+	if (!MXC_NFC_NO_IP_REG) {
+		r = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+		if (!r) {
+			error = -ENXIO;
+			goto out_1;
+		}
+	}
+	nfc_ip_base = ioremap(r->start, resource_size(r));
+
+	r = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!r) {
+		error = -ENXIO;
+		goto out_2;
+	}
+	nfc_irq = r->start;
+
+	init_waitqueue_head(&irq_waitq);
+	error = request_irq(nfc_irq, mxc_nfc_irq, 0, "mxc_nd", NULL);
+	if (error)
+		goto out_3;
+
+	return 0;
+out_3:
+out_2:
+	if (!MXC_NFC_NO_IP_REG)
+		iounmap(nfc_ip_base);
+out_1:
+	iounmap(nfc_axi_base);
+out_0:
+	return error;
+}
+
 static void mxc_nfc_init(void)
 {
 	/* Disable interrupt */
@@ -1137,11 +1286,13 @@ static void mxc_nfc_init(void)
 	/* Unlock the internal RAM Buffer */
 	raw_write(NFC_SET_BLS(NFC_BLS_UNLCOKED), REG_NFC_BLS);
 
-	/* Blocks to be unlocked */
-	UNLOCK_ADDR(0x0, 0xFFFF);
+	if (!(cpu_is_mx53())) {
+		/* Blocks to be unlocked */
+		UNLOCK_ADDR(0x0, 0xFFFF);
 
-	/* Unlock Block Command for given address range */
-	raw_write(NFC_SET_WPC(NFC_WPC_UNLOCK), REG_NFC_WPC);
+		/* Unlock Block Command for given address range */
+		raw_write(NFC_SET_WPC(NFC_WPC_UNLOCK), REG_NFC_WPC);
+	}
 
 	/* Enable symetric mode by default except mx37TO1.0 */
 	if (!(cpu_is_mx37_rev(CHIP_REV_1_0) == 1))
@@ -1232,6 +1383,81 @@ int nand_scan_mid(struct mtd_info *mtd)
 	return 0;
 }
 
+/*!
+ * show_device_disable_bi_swap()
+ * Shows the value of the 'disable_bi_swap' flag.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer that will receive a representation of the attribute.
+ */
+static ssize_t show_device_disable_bi_swap(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d\n", mxc_nand_data->disable_bi_swap);
+}
+
+/*!
+ * store_device_disable_bi_swap()
+ * Sets the value of the 'disable_bi_swap' flag.
+ *
+ * @dev:   The device of interest.
+ * @attr:  The attribute of interest.
+ * @buf:   A buffer containing a new attribute value.
+ * @size:  The size of the buffer.
+ */
+static ssize_t store_device_disable_bi_swap(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	const char *p = buf;
+	unsigned long v;
+
+	/* Try to make sense of what arrived from user space. */
+
+	if (strict_strtoul(p, 0, &v) < 0)
+		return size;
+
+	if (v > 0)
+		v = 1;
+	mxc_nand_data->disable_bi_swap = v;
+	return size;
+
+}
+
+static DEVICE_ATTR(disable_bi_swap, 0644,
+	show_device_disable_bi_swap, store_device_disable_bi_swap);
+static struct device_attribute *device_attributes[] = {
+	&dev_attr_disable_bi_swap,
+};
+/*!
+ * manage_sysfs_files() - Creates/removes sysfs files for this device.
+ *
+ * @create: create/remove the sys entry.
+ */
+static void manage_sysfs_files(int create)
+{
+	struct device *dev = mxc_nand_data->dev;
+	int error;
+	unsigned int i;
+	struct device_attribute **attr;
+
+	for (i = 0, attr = device_attributes;
+			i < ARRAY_SIZE(device_attributes); i++, attr++) {
+
+		if (create) {
+			error = device_create_file(dev, *attr);
+			if (error) {
+				while (--attr >= device_attributes)
+					device_remove_file(dev, *attr);
+				return;
+			}
+		} else {
+			device_remove_file(dev, *attr);
+		}
+	}
+
+}
+
 
 /*!
  * This function is called during the driver binding process.
@@ -1249,8 +1475,10 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 	struct flash_platform_data *flash = pdev->dev.platform_data;
 	int nr_parts = 0, err = 0;
 
-	nfc_axi_base = IO_ADDRESS(NFC_AXI_BASE_ADDR);
-	nfc_ip_base = IO_ADDRESS(NFC_BASE_ADDR);
+	/* get the resource */
+	err = mxc_get_resources(pdev);
+	if (err)
+		goto out;
 
 	/* init the nfc */
 	mxc_nfc_init();
@@ -1298,12 +1526,7 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 
 	nfc_clk = clk_get(&pdev->dev, "nfc_clk");
 	clk_enable(nfc_clk);
-
-	init_waitqueue_head(&irq_waitq);
-	err = request_irq(MXC_INT_NANDFC, mxc_nfc_irq, 0, "mxc_nd", NULL);
-	if (err) {
-		goto out_1;
-	}
+	mxc_nand_data->clk_active = 1;
 
 	if (hardware_ecc) {
 		this->ecc.read_page = mxc_nand_read_page;
@@ -1359,6 +1582,16 @@ static int __init mxcnd_probe(struct platform_device *pdev)
 		add_mtd_device(mtd);
 	}
 
+#ifdef CONFIG_MODULE_CCXMX51
+        {
+                extern u8 ccwmx51_swap_bi;
+                mxc_nand_data->disable_bi_swap = !ccwmx51_swap_bi;
+                pr_info("%sUsing swap BI (%x)\n", ccwmx51_swap_bi ? "" : "No ", ccwmx51_swap_bi);
+        }
+#endif
+	/* Create sysfs entries for this device. */
+	manage_sysfs_files(true);
+
 	platform_set_drvdata(pdev, mtd);
 
 	return 0;
@@ -1386,15 +1619,16 @@ static int __exit mxcnd_remove(struct platform_device *pdev)
 	if (flash->exit)
 		flash->exit();
 
+	manage_sysfs_files(false);
 	mxc_free_buf();
 
-	clk_disable(nfc_clk);
+	mxc_nand_clk_disable();
 	clk_put(nfc_clk);
 	platform_set_drvdata(pdev, NULL);
 
 	if (mxc_nand_data) {
 		nand_release(mtd);
-		free_irq(MXC_INT_NANDFC, NULL);
+		free_irq(nfc_irq, NULL);
 		kfree(mxc_nand_data);
 	}
 
@@ -1419,7 +1653,7 @@ static int mxcnd_suspend(struct platform_device *pdev, pm_message_t state)
 	DEBUG(MTD_DEBUG_LEVEL0, "MXC_ND2 : NAND suspend\n");
 
 	/* Disable the NFC clock */
-	clk_disable(nfc_clk);
+	mxc_nand_clk_disable();
 
 	return 0;
 }
@@ -1438,7 +1672,7 @@ static int mxcnd_resume(struct platform_device *pdev)
 	DEBUG(MTD_DEBUG_LEVEL0, "MXC_ND2 : NAND resume\n");
 
 	/* Enable the NFC clock */
-	clk_enable(nfc_clk);
+	mxc_nand_clk_enable();
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/mxc_nd2.h b/drivers/mtd/nand/mxc_nd2.h
index ea128f6da41b..e8ef125ce8e7 100644
--- a/drivers/mtd/nand/mxc_nd2.h
+++ b/drivers/mtd/nand/mxc_nd2.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -32,16 +32,37 @@
 #define IS_LARGE_PAGE_NAND      ((mtd->writesize / num_of_interleave) > 512)
 
 #define GET_NAND_OOB_SIZE	(mtd->oobsize / num_of_interleave)
+#define GET_NAND_PAGE_SIZE      (mtd->writesize / num_of_interleave)
 
 #define NAND_PAGESIZE_2KB	2048
 #define NAND_PAGESIZE_4KB	4096
 
+/*
+ * main area for bad block marker is in the last data section
+ * the spare area for swapped bad block marker is the second
+ * byte of last spare section
+ */
+#define NAND_SECTIONS        (GET_NAND_PAGE_SIZE >> 9)
+#define NAND_OOB_PER_SECTION (((GET_NAND_OOB_SIZE / NAND_SECTIONS) >> 1) << 1)
+#define NAND_CHUNKS	     (GET_NAND_PAGE_SIZE / (512 + NAND_OOB_PER_SECTION))
+
+#define BAD_BLK_MARKER_MAIN_OFFS \
+	(GET_NAND_PAGE_SIZE - NAND_CHUNKS * NAND_OOB_PER_SECTION)
+
+#define BAD_BLK_MARKER_SP_OFFS (NAND_CHUNKS * SPARE_LEN)
+
+#define BAD_BLK_MARKER_OOB_OFFS (NAND_CHUNKS * NAND_OOB_PER_SECTION)
+
+#define BAD_BLK_MARKER_MAIN  \
+	((u32)MAIN_AREA0 + BAD_BLK_MARKER_MAIN_OFFS)
+
+#define BAD_BLK_MARKER_SP  \
+	((u32)SPARE_AREA0 + BAD_BLK_MARKER_SP_OFFS)
+
 #ifdef CONFIG_ARCH_MXC_HAS_NFC_V3
 /*
  * For V3 NFC registers Definition
  */
-/* AXI Bus Mapped */
-#define NFC_AXI_BASE_ADDR		MX51_NFC_BASE_ADDR_AXI
 
 #if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_1)	/* mx37 */
 #define MXC_INT_NANDFC			MXC_INT_EMI
@@ -106,13 +127,6 @@
 #define NFC_SPAS_WIDTH 8
 #define NFC_SPAS_SHIFT 16
 
-#define IS_4BIT_ECC \
-( \
-	cpu_is_mx51_rev(CHIP_REV_2_0) > 0 ? \
-		!((raw_read(NFC_CONFIG2) & NFC_ECC_MODE_4) >> 6) : \
-		((raw_read(NFC_CONFIG2) & NFC_ECC_MODE_4) >> 6) \
-)
-
 #define NFC_SET_SPAS(v)			\
 	raw_write((((raw_read(NFC_CONFIG2) & \
 	NFC_FIELD_RESET(NFC_SPAS_WIDTH, NFC_SPAS_SHIFT)) | ((v) << 16))), \
@@ -120,24 +134,32 @@
 
 #define NFC_SET_ECC_MODE(v)		\
 do { \
-	if (cpu_is_mx51_rev(CHIP_REV_2_0) > 0) { \
+	if (cpu_is_mx53() > 0) { \
 		if ((v) == NFC_SPAS_218 || (v) == NFC_SPAS_112) \
 			raw_write(((raw_read(NFC_CONFIG2) & \
-					NFC_ECC_MODE_MASK) | \
-					NFC_ECC_MODE_4), NFC_CONFIG2); \
+					~(3 << 6)) | \
+					NFC_ECC_MODE_16), NFC_CONFIG2); \
 		else \
 			raw_write(((raw_read(NFC_CONFIG2) & \
-					NFC_ECC_MODE_MASK) & \
+					~(3 << 6)) & \
+					NFC_ECC_MODE_4), NFC_CONFIG2); \
+	} else if (cpu_is_mx51_rev(CHIP_REV_2_0) > 0) { \
+		if ((v) == NFC_SPAS_218 || (v) == NFC_SPAS_112) \
+			raw_write(((raw_read(NFC_CONFIG2) & \
+					~(1 << 6)) | \
 					NFC_ECC_MODE_8), NFC_CONFIG2); \
+		else \
+			raw_write(((raw_read(NFC_CONFIG2) & \
+					~(1 << 6)) & \
+                                        ~NFC_ECC_MODE_8), NFC_CONFIG2); \
 	} else { \
 		if ((v) == NFC_SPAS_218 || (v) == NFC_SPAS_112) \
 			raw_write(((raw_read(NFC_CONFIG2) & \
-					NFC_ECC_MODE_MASK) & \
-					NFC_ECC_MODE_8), NFC_CONFIG2); \
+					~(1 << 6))), NFC_CONFIG2); \
 		else \
 			raw_write(((raw_read(NFC_CONFIG2) & \
-					NFC_ECC_MODE_MASK) | \
-					NFC_ECC_MODE_4), NFC_CONFIG2); \
+					~(1 << 6)) | \
+                                        NFC_ECC_MODE_8), NFC_CONFIG2); \
 	} \
 } while (0)
 
@@ -151,7 +173,6 @@ do { \
 	} while(0)
 
 #else
-#define IS_4BIT_ECC			1
 #define NFC_SET_SPAS(v)
 #define NFC_SET_ECC_MODE(v)
 #define NFC_SET_NFMS(v)	(NFMS |= (v))
@@ -292,9 +313,10 @@ do { \
 #define NFC_WPC_RESET			~(7)
 #if defined(CONFIG_ARCH_MXC_HAS_NFC_V3_1) || \
     defined(CONFIG_ARCH_MXC_HAS_NFC_V3_2)
-#define NFC_ECC_MODE_4    		(1 << 6)
-#define NFC_ECC_MODE_8			~(1 << 6)
-#define NFC_ECC_MODE_MASK 		~(1 << 6)
+#define NFC_ECC_MODE_4    		(0x0 << 6)
+#define NFC_ECC_MODE_8			(0x1 << 6)
+#define NFC_ECC_MODE_14                 (0x3 << 6)
+#define NFC_ECC_MODE_16                 (0x3 << 6)
 #define NFC_SPAS_16			8
 #define NFC_SPAS_64		 	32
 #define NFC_SPAS_128			64
@@ -454,7 +476,8 @@ do {	\
 		NFC_SET_ST_CMD(0x70); \
 		raw_write(raw_read(NFC_CONFIG3) | NFC_NO_SDMA, NFC_CONFIG3); \
 		raw_write(raw_read(NFC_CONFIG3) | NFC_RBB_MODE, NFC_CONFIG3); \
-		SET_NFC_DELAY_LINE(0); \
+		if (cpu_is_mx51()) \
+			SET_NFC_DELAY_LINE(0); \
 	} \
 } while (0)
 #endif
@@ -472,14 +495,13 @@ do {	\
  * For V1/V2 NFC registers Definition
  */
 
-#define NFC_AXI_BASE_ADDR      		0x00
 /*
  * Addresses for NFC registers
  */
 #ifdef CONFIG_ARCH_MXC_HAS_NFC_V2_1
-#define NFC_REG_BASE			(nfc_ip_base + 0x1000)
+#define NFC_REG_BASE			(nfc_axi_base + 0x1000)
 #else
-#define NFC_REG_BASE			nfc_ip_base
+#define NFC_REG_BASE			nfc_axi_base
 #endif
 #define NFC_BUF_SIZE            	(NFC_REG_BASE + 0xE00)
 #define NFC_BUF_ADDR            	(NFC_REG_BASE + 0xE04)
@@ -517,18 +539,18 @@ do {	\
 /*!
  * Addresses for NFC RAM BUFFER Main area 0
  */
-#define MAIN_AREA0      		(u16 *)(nfc_ip_base + 0x000)
-#define MAIN_AREA1      		(u16 *)(nfc_ip_base + 0x200)
+#define MAIN_AREA0      		(u16 *)(nfc_axi_base + 0x000)
+#define MAIN_AREA1      		(u16 *)(nfc_axi_base + 0x200)
 
 /*!
  * Addresses for NFC SPARE BUFFER Spare area 0
  */
 #ifdef CONFIG_ARCH_MXC_HAS_NFC_V2_1
-#define SPARE_AREA0     		(u16 *)(nfc_ip_base + 0x1000)
+#define SPARE_AREA0     		(u16 *)(nfc_axi_base + 0x1000)
 #define SPARE_LEN			64
 #define SPARE_COUNT			8
 #else
-#define SPARE_AREA0     		(u16 *)(nfc_ip_base + 0x800)
+#define SPARE_AREA0     		(u16 *)(nfc_axi_base + 0x800)
 #define SPARE_LEN       		16
 #define SPARE_COUNT     		4
 #endif
@@ -539,8 +561,6 @@ do {	\
 #define SPAS_SHIFT		(0)
 #define REG_NFC_SPAS NFC_SPAS
 #define SPAS_MASK	(0xFF00)
-#define IS_4BIT_ECC			\
-	((raw_read(REG_NFC_ECC_MODE) & NFC_ECC_MODE_4) >> 0)
 
 #define NFC_SET_SPAS(v)			\
 	raw_write(((raw_read(REG_NFC_SPAS) & SPAS_MASK) | ((v<<SPAS_SHIFT))), \
@@ -578,7 +598,6 @@ do { \
 	} \
 } while (0)
 #else
-#define IS_4BIT_ECC			(1)
 #define NFC_SET_SPAS(v)
 #define NFC_SET_ECC_MODE(v)
 #define GET_ECC_STATUS()  raw_read(REG_NFC_ECC_STATUS_RESULT);
diff --git a/drivers/mtd/nand/nand_device_info.c b/drivers/mtd/nand/nand_device_info.c
index ecd5b21189cc..1ab1d1d21811 100644
--- a/drivers/mtd/nand/nand_device_info.c
+++ b/drivers/mtd/nand/nand_device_info.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -1284,9 +1284,9 @@ static struct nand_device_info nand_device_info_table_type_9[] __initdata =
 	.data_hold_in_ns          = 10,
 	.address_setup_in_ns      = 25,
 	.gpmi_sample_delay_in_ns  = 6,
-	.tREA_in_ns               = -1,
-	.tRLOH_in_ns              = -1,
-	.tRHOH_in_ns              = -1,
+	.tREA_in_ns               = 20,
+	.tRLOH_in_ns              = 5,
+	.tRHOH_in_ns              = 15,
 	"K9LBG08U0D",
 	},
 	{
diff --git a/drivers/mxc/Kconfig b/drivers/mxc/Kconfig
index 6e67087d2efa..b26c1dc11ad6 100644
--- a/drivers/mxc/Kconfig
+++ b/drivers/mxc/Kconfig
@@ -33,6 +33,7 @@ source "drivers/mxc/bt/Kconfig"
 source "drivers/mxc/gps_ioctrl/Kconfig"
 source "drivers/mxc/mlb/Kconfig"
 source "drivers/mxc/adc/Kconfig"
+source "drivers/mxc/amd-gpu/Kconfig"
 
 endmenu
 
diff --git a/drivers/mxc/Makefile b/drivers/mxc/Makefile
index 6416bc429888..5193fa50eb9f 100644
--- a/drivers/mxc/Makefile
+++ b/drivers/mxc/Makefile
@@ -15,3 +15,4 @@ obj-$(CONFIG_MXC_BLUETOOTH)		+= bt/
 obj-$(CONFIG_GPS_IOCTRL)		+= gps_ioctrl/
 obj-$(CONFIG_MXC_MLB)                   += mlb/
 obj-$(CONFIG_IMX_ADC)                   += adc/
+obj-$(CONFIG_MXC_AMD_GPU)                   += amd-gpu/
diff --git a/drivers/mxc/ipu/ipu_common.c b/drivers/mxc/ipu/ipu_common.c
index 43ba100b5c5d..a1dc566e7f8f 100644
--- a/drivers/mxc/ipu/ipu_common.c
+++ b/drivers/mxc/ipu/ipu_common.c
@@ -707,6 +707,38 @@ int32_t ipu_select_buffer(ipu_channel_t channel, ipu_buffer_t type,
 }
 
 /*!
+ * This function check buffer ready for a logical channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to clear.
+ *
+ * @param       bufNum          Input parameter for which buffer number clear
+ * 				ready state.
+ *
+ */
+int32_t ipu_check_buffer_busy(ipu_channel_t channel, ipu_buffer_t type,
+		uint32_t bufNum)
+{
+	uint32_t dma_chan = channel_2_dma(channel, type);
+	uint32_t reg;
+
+	if (dma_chan == IDMA_CHAN_INVALID)
+		return -EINVAL;
+
+	if (bufNum == 0)
+		reg = __raw_readl(IPU_CHA_BUF0_RDY);
+	else
+		reg = __raw_readl(IPU_CHA_BUF1_RDY);
+
+	if (reg & (1UL << dma_chan))
+		return 1;
+	else
+		return 0;
+}
+EXPORT_SYMBOL(ipu_check_buffer_busy);
+
+/*!
  * This function links 2 channels together for automatic frame
  * synchronization. The output of the source channel is linked to the input of
  * the destination channel.
diff --git a/drivers/mxc/ipu/ipu_csi.c b/drivers/mxc/ipu/ipu_csi.c
index 10708cf3ba54..58d58c10af51 100644
--- a/drivers/mxc/ipu/ipu_csi.c
+++ b/drivers/mxc/ipu/ipu_csi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2005-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -90,10 +90,13 @@ ipu_csi_init_interface(uint16_t width, uint16_t height, uint32_t pixel_fmt,
 	__raw_writel(height << 16 | 0x22, CSI_FLASH_STROBE_2);
 
 	/* Set CCIR registers */
-	if ((sig.clk_mode == IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE) ||
-	    (sig.clk_mode == IPU_CSI_CLK_MODE_CCIR656_INTERLACED)) {
+	if (sig.clk_mode == IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE) {
 		__raw_writel(0x40030, CSI_CCIR_CODE_1);
 		__raw_writel(0xFF0000, CSI_CCIR_CODE_3);
+	} else if (sig.clk_mode == IPU_CSI_CLK_MODE_CCIR656_INTERLACED) {
+		__raw_writel(0xD07DF, CSI_CCIR_CODE_1);
+		__raw_writel(0x40596, CSI_CCIR_CODE_2);
+		__raw_writel(0xFF0000, CSI_CCIR_CODE_3);
 	}
 
 	dev_dbg(g_ipu_dev, "CSI_SENS_CONF = 0x%08X\n",
diff --git a/drivers/mxc/ipu/ipu_device.c b/drivers/mxc/ipu/ipu_device.c
index 5fd1c51ec9b1..713ba2005ae9 100644
--- a/drivers/mxc/ipu/ipu_device.c
+++ b/drivers/mxc/ipu/ipu_device.c
@@ -169,6 +169,7 @@ static int mxc_ipu_ioctl(struct inode *inode, struct file *file,
 			     sizeof(ipu_channel_buf_parm))) {
 				return -EFAULT;
 			}
+
 			ret =
 			    ipu_init_channel_buffer(parm.channel, parm.type,
 						    parm.pixel_fmt,
diff --git a/drivers/mxc/ipu/ipu_ic.c b/drivers/mxc/ipu/ipu_ic.c
index cdf823a2760b..9fe087590368 100644
--- a/drivers/mxc/ipu/ipu_ic.c
+++ b/drivers/mxc/ipu/ipu_ic.c
@@ -71,8 +71,7 @@ void _ipu_ic_enable_task(ipu_channel_t channel)
 	case MEM_ROT_PP_MEM:
 		ic_conf |= IC_CONF_PP_ROT_EN;
 		break;
-	case CSI_MEM:
-		// ???
+	case CSI_MEM1:
 		ic_conf |= IC_CONF_RWS_EN | IC_CONF_PRPENC_EN;
 		break;
 	default:
@@ -110,8 +109,7 @@ void _ipu_ic_disable_task(ipu_channel_t channel)
 	case MEM_ROT_PP_MEM:
 		ic_conf &= ~IC_CONF_PP_ROT_EN;
 		break;
-	case CSI_MEM:
-		// ???
+	case CSI_MEM1:
 		ic_conf &= ~(IC_CONF_RWS_EN | IC_CONF_PRPENC_EN);
 		break;
 	default:
diff --git a/drivers/mxc/ipu/pf/mxc_pf.c b/drivers/mxc/ipu/pf/mxc_pf.c
index 744152415e3a..8abffb4d8d44 100644
--- a/drivers/mxc/ipu/pf/mxc_pf.c
+++ b/drivers/mxc/ipu/pf/mxc_pf.c
@@ -108,6 +108,7 @@ static int mxc_pf_init(pf_init_params * pf_init)
 
 	memset(&params, 0, sizeof(params));
 	params.mem_pf_mem.operation = pf_data.mode;
+
 	err = ipu_init_channel(MEM_PF_Y_MEM, &params);
 	if (err < 0) {
 		printk(KERN_ERR "mxc_pf: error initializing channel\n");
diff --git a/drivers/mxc/ipu3/ipu_calc_stripes_sizes.c b/drivers/mxc/ipu3/ipu_calc_stripes_sizes.c
index 6a29c90fe095..5d5e0b9155a0 100644
--- a/drivers/mxc/ipu3/ipu_calc_stripes_sizes.c
+++ b/drivers/mxc/ipu3/ipu_calc_stripes_sizes.c
@@ -260,7 +260,7 @@ int ipu_calc_stripes_sizes(const unsigned int input_frame_width,
 				input_frame_width >> 1;
 			left->output_width = right->output_width = right->output_column =
 				output_frame_width >> 1;
-			left->input_column = right->input_column = 0;
+			left->input_column = 0;
 			div = _do_div(((((u64)irr_steps) << 32) *
 				       (right->input_width - 1)), (right->output_width - 1));
 			left->irr = right->irr = truncate(0, div, 1);
diff --git a/drivers/mxc/ipu3/ipu_capture.c b/drivers/mxc/ipu3/ipu_capture.c
index 5d084ab37b0b..b9967135eac1 100644
--- a/drivers/mxc/ipu3/ipu_capture.c
+++ b/drivers/mxc/ipu3/ipu_capture.c
@@ -26,6 +26,7 @@
 #include <linux/delay.h>
 #include <linux/ipu.h>
 #include <linux/clk.h>
+#include <mach/mxc_dvfs.h>
 
 #include "ipu_prv.h"
 #include "ipu_regs.h"
@@ -93,6 +94,12 @@ ipu_csi_init_interface(uint16_t width, uint16_t height, uint32_t pixel_fmt,
 		cfg_param.force_eof << CSI_SENS_CONF_FORCE_EOF_SHIFT |
 		cfg_param.data_en_pol << CSI_SENS_CONF_DATA_EN_POL_SHIFT;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	__raw_writel(data, CSI_SENS_CONF(csi));
@@ -101,11 +108,18 @@ ipu_csi_init_interface(uint16_t width, uint16_t height, uint32_t pixel_fmt,
 	__raw_writel((width - 1) | (height - 1) << 16, CSI_SENS_FRM_SIZE(csi));
 
 	/* Set CCIR registers */
-	if ((cfg_param.clk_mode == IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE) ||
-	    (cfg_param.clk_mode == IPU_CSI_CLK_MODE_CCIR656_INTERLACED)) {
-		_ipu_csi_ccir_err_detection_enable(csi);
+	if (cfg_param.clk_mode == IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE) {
 		__raw_writel(0x40030, CSI_CCIR_CODE_1(csi));
 		__raw_writel(0xFF0000, CSI_CCIR_CODE_3(csi));
+	} else if (cfg_param.clk_mode == IPU_CSI_CLK_MODE_CCIR656_INTERLACED) {
+		_ipu_csi_ccir_err_detection_enable(csi);
+		/* Field0BlankEnd = 0x7, Field0BlankStart = 0x3,
+		   Field0ActiveEnd = 0x5, Field0ActiveStart = 0x1 */
+		__raw_writel(0xD07DF, CSI_CCIR_CODE_1(csi));
+		/* Field1BlankEnd = 0x6, Field1BlankStart = 0x2,
+		   Field1ActiveEnd = 0x4, Field1ActiveStart = 0 */
+		__raw_writel(0x40596, CSI_CCIR_CODE_2(csi));
+		__raw_writel(0xFF0000, CSI_CCIR_CODE_3(csi));
 	} else if ((cfg_param.clk_mode ==
 			IPU_CSI_CLK_MODE_CCIR1120_PROGRESSIVE_DDR) ||
 		(cfg_param.clk_mode ==
@@ -171,34 +185,13 @@ int _ipu_csi_mclk_set(uint32_t pixel_clk, uint32_t csi)
  */
 int ipu_csi_enable_mclk(int csi, bool flag, bool wait)
 {
-	struct clk *clk;
 
 	if (flag) {
-		if (cpu_is_mx53()) {
-			if (csi == 0) {
-				clk = clk_get(NULL, "ssi_ext1_clk");
-				clk_enable(clk);
-				clk_put(clk);
-			} else {
-				pr_err("invalid csi num %d\n", csi);
-				return -EINVAL;
-			}
-		} else
-			clk_enable(g_csi_clk[csi]);
+		clk_enable(g_csi_clk[csi]);
 		if (wait == true)
 			msleep(10);
 	} else {
-		if (cpu_is_mx53()) {
-			if (csi == 0) {
-				clk = clk_get(NULL, "ssi_ext1_clk");
-				clk_disable(clk);
-				clk_put(clk);
-			} else {
-				pr_err("invalid csi num %d\n", csi);
-				return -EINVAL;
-			}
-		} else
-			clk_disable(g_csi_clk[csi]);
+		clk_disable(g_csi_clk[csi]);
 	}
 
 	return 0;
@@ -217,6 +210,12 @@ void ipu_csi_get_window_size(uint32_t *width, uint32_t *height, uint32_t csi)
 	uint32_t reg;
 	unsigned long lock_flags;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	reg = __raw_readl(CSI_ACT_FRM_SIZE(csi));
@@ -238,6 +237,12 @@ void ipu_csi_set_window_size(uint32_t width, uint32_t height, uint32_t csi)
 {
 	unsigned long lock_flags;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	__raw_writel((width - 1) | (height - 1) << 16, CSI_ACT_FRM_SIZE(csi));
@@ -258,6 +263,12 @@ void ipu_csi_set_window_pos(uint32_t left, uint32_t top, uint32_t csi)
 	uint32_t temp;
 	unsigned long lock_flags;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
@@ -280,6 +291,12 @@ void _ipu_csi_horizontal_downsize_enable(uint32_t csi)
 	uint32_t temp;
 	unsigned long lock_flags;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
@@ -300,6 +317,12 @@ void _ipu_csi_horizontal_downsize_disable(uint32_t csi)
 	uint32_t temp;
 	unsigned long lock_flags;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
@@ -320,6 +343,12 @@ void _ipu_csi_vertical_downsize_enable(uint32_t csi)
 	uint32_t temp;
 	unsigned long lock_flags;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
@@ -340,6 +369,12 @@ void _ipu_csi_vertical_downsize_disable(uint32_t csi)
 	uint32_t temp;
 	unsigned long lock_flags;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(CSI_OUT_FRM_CTRL(csi));
@@ -366,6 +401,12 @@ void ipu_csi_set_test_generator(bool active, uint32_t r_value,
 	uint32_t temp;
 	unsigned long lock_flags;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(CSI_TST_CTRL(csi));
@@ -401,6 +442,12 @@ void _ipu_csi_ccir_err_detection_enable(uint32_t csi)
 {
 	uint32_t temp;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	temp = __raw_readl(CSI_CCIR_CODE_1(csi));
 	temp |= CSI_CCIR_ERR_DET_EN;
 	__raw_writel(temp, CSI_CCIR_CODE_1(csi));
@@ -417,6 +464,12 @@ void _ipu_csi_ccir_err_detection_disable(uint32_t csi)
 {
 	uint32_t temp;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	temp = __raw_readl(CSI_CCIR_CODE_1(csi));
 	temp &= ~CSI_CCIR_ERR_DET_EN;
 	__raw_writel(temp, CSI_CCIR_CODE_1(csi));
@@ -442,6 +495,12 @@ int _ipu_csi_set_mipi_di(uint32_t num, uint32_t di_val, uint32_t csi)
 		goto err;
 	}
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(CSI_MIPI_DI(csi));
@@ -499,6 +558,12 @@ int _ipu_csi_set_skip_isp(uint32_t skip, uint32_t max_ratio, uint32_t csi)
 		goto err;
 	}
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(CSI_SKIP(csi));
@@ -536,6 +601,12 @@ int _ipu_csi_set_skip_smfc(uint32_t skip, uint32_t max_ratio,
 		goto err;
 	}
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(CSI_SKIP(csi));
@@ -585,7 +656,6 @@ void _ipu_smfc_init(ipu_channel_t channel, uint32_t mipi_id, uint32_t csi)
 	default:
 		return;
 	}
-
 	__raw_writel(temp, SMFC_MAP);
 }
 
@@ -604,6 +674,12 @@ void _ipu_smfc_set_wmc(ipu_channel_t channel, bool set, uint32_t level)
 	uint32_t temp;
 	unsigned long lock_flags;
 
+	if (g_ipu_clk_enabled == false) {
+		stop_dvfs_per();
+		g_ipu_clk_enabled = true;
+		clk_enable(g_ipu_clk);
+	}
+
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
 	temp = __raw_readl(SMFC_WMC);
diff --git a/drivers/mxc/ipu3/ipu_common.c b/drivers/mxc/ipu3/ipu_common.c
index 68be542e8370..b1b8a8b39ae1 100644
--- a/drivers/mxc/ipu3/ipu_common.c
+++ b/drivers/mxc/ipu3/ipu_common.c
@@ -52,8 +52,8 @@ unsigned char g_dc_di_assignment[10];
 ipu_channel_t g_ipu_csi_channel[2];
 int g_ipu_irq[2];
 int g_ipu_hw_rev;
-bool g_sec_chan_en[22];
-bool g_thrd_chan_en[21];
+bool g_sec_chan_en[24];
+bool g_thrd_chan_en[24];
 uint32_t g_channel_init_mask;
 uint32_t g_channel_enable_mask;
 DEFINE_SPINLOCK(ipu_lock);
@@ -354,8 +354,8 @@ static int ipu_probe(struct platform_device *pdev)
 	g_di_clk[0] = plat_data->di_clk[0];
 	g_di_clk[1] = plat_data->di_clk[1];
 
-	g_csi_clk[0] = clk_get(&pdev->dev, "csi_mclk1");
-	g_csi_clk[1] = clk_get(&pdev->dev, "csi_mclk2");
+	g_csi_clk[0] = plat_data->csi_clk[0];
+	g_csi_clk[1] = plat_data->csi_clk[1];
 
 	__raw_writel(0x807FFFFF, IPU_MEM_RST);
 	while (__raw_readl(IPU_MEM_RST) & 0x80000000) ;
@@ -372,7 +372,7 @@ static int ipu_probe(struct platform_device *pdev)
 	_ipu_dmfc_init(DMFC_NORMAL, 1);
 
 	/* Set sync refresh channels and CSI->mem channel as high priority */
-	__raw_writel(0x18800001L, IDMAC_CHA_PRI(0));
+	__raw_writel(0x18800003L, IDMAC_CHA_PRI(0));
 
 	/* Set MCU_T to divide MCU access window into 2 */
 	__raw_writel(0x00400000L | (IPU_MCU_T_DEFAULT << 18), IPU_DISP_GEN);
@@ -415,7 +415,10 @@ int ipu_remove(struct platform_device *pdev)
 
 void ipu_dump_registers(void)
 {
+	printk(KERN_DEBUG "--------------------------------------------\n");
 	printk(KERN_DEBUG "IPU_CONF = \t0x%08X\n", __raw_readl(IPU_CONF));
+	printk(KERN_DEBUG "SMFC_MAP = \t0x%08X\n", __raw_readl(SMFC_MAP));
+	printk(KERN_DEBUG "SMFC_WMC = \t0x%08X\n", __raw_readl(SMFC_WMC));
 	printk(KERN_DEBUG "IDMAC_CONF = \t0x%08X\n", __raw_readl(IDMAC_CONF));
 	printk(KERN_DEBUG "IDMAC_CHA_EN1 = \t0x%08X\n",
 	       __raw_readl(IDMAC_CHA_EN(0)));
@@ -451,6 +454,25 @@ void ipu_dump_registers(void)
 	       __raw_readl(IPU_FS_PROC_FLOW3));
 	printk(KERN_DEBUG "IPU_FS_DISP_FLOW1 = \t0x%08X\n",
 	       __raw_readl(IPU_FS_DISP_FLOW1));
+
+	printk(KERN_DEBUG "IPU_INT_CTRL_1 = \t0x%08X\n", __raw_readl(IPU_INT_CTRL (1)));
+	printk(KERN_DEBUG "IPU_INT_STAT_1 = \t0x%08X\n", __raw_readl(IPU_INT_STAT (1)));
+	printk(KERN_DEBUG "IPU_INT_STAT_2 = \t0x%08X\n", __raw_readl(IPU_INT_STAT (2)));
+	printk(KERN_DEBUG "IPU_INT_STAT_3 = \t0x%08X\n", __raw_readl(IPU_INT_STAT (3)));
+	printk(KERN_DEBUG "IPU_INT_STAT_4 = \t0x%08X\n", __raw_readl(IPU_INT_STAT (4)));
+	printk(KERN_DEBUG "IPU_INT_STAT_5 = \t0x%08X\n", __raw_readl(IPU_INT_STAT (5)));
+	printk(KERN_DEBUG "IPU_INT_STAT_6 = \t0x%08X\n", __raw_readl(IPU_INT_STAT (6)));
+
+	printk(KERN_DEBUG "CSI0_SENS_CONF = \t0x%08X\n", __raw_readl(CSI_SENS_CONF (0)));
+	printk(KERN_DEBUG "CSI0_SENS_FRM_SIZE = \t0x%08X\n", __raw_readl(CSI_SENS_FRM_SIZE(0)));
+	printk(KERN_DEBUG "CSI0_ACT_FRM_SIZE = \t0x%08X\n", __raw_readl(CSI_ACT_FRM_SIZE(0)));
+	printk(KERN_DEBUG "CSI0_SKIP = \t0x%08X\n", __raw_readl(CSI_SKIP(0)));
+
+	printk(KERN_DEBUG "CSI1_SENS_CONF = \t0x%08X\n", __raw_readl(CSI_SENS_CONF (1)));
+	printk(KERN_DEBUG "CSI1_SENS_FRM_SIZE = \t0x%08X\n", __raw_readl(CSI_SENS_FRM_SIZE(1)));
+	printk(KERN_DEBUG "CSI1_ACT_FRM_SIZE = \t0x%08X\n", __raw_readl(CSI_ACT_FRM_SIZE(1)));
+	printk(KERN_DEBUG "CSI1_SKIP = \t0x%08X\n", __raw_readl(CSI_SKIP(1)));
+	printk(KERN_DEBUG "--------------------------------------------\n");
 }
 
 /*!
@@ -660,7 +682,8 @@ int32_t ipu_init_channel(ipu_channel_t channel, ipu_channel_params_t *params)
 
 		g_dc_di_assignment[1] = params->mem_dc_sync.di;
 		_ipu_dc_init(1, params->mem_dc_sync.di,
-			     params->mem_dc_sync.interlaced);
+			     params->mem_dc_sync.interlaced,
+			     params->mem_dc_sync.out_pixel_fmt);
 		ipu_di_use_count[params->mem_dc_sync.di]++;
 		ipu_dc_use_count++;
 		ipu_dmfc_use_count++;
@@ -678,7 +701,8 @@ int32_t ipu_init_channel(ipu_channel_t channel, ipu_channel_params_t *params)
 		_ipu_dp_init(channel, params->mem_dp_bg_sync.in_pixel_fmt,
 			     params->mem_dp_bg_sync.out_pixel_fmt);
 		_ipu_dc_init(5, params->mem_dp_bg_sync.di,
-			     params->mem_dp_bg_sync.interlaced);
+			     params->mem_dp_bg_sync.interlaced,
+			     params->mem_dp_bg_sync.out_pixel_fmt);
 		ipu_di_use_count[params->mem_dp_bg_sync.di]++;
 		ipu_dc_use_count++;
 		ipu_dp_use_count++;
@@ -702,7 +726,7 @@ int32_t ipu_init_channel(ipu_channel_t channel, ipu_channel_params_t *params)
 		}
 
 		g_dc_di_assignment[8] = params->direct_async.di;
-		_ipu_dc_init(8, params->direct_async.di, false);
+		_ipu_dc_init(8, params->direct_async.di, false, IPU_PIX_FMT_GENERIC);
 		ipu_di_use_count[params->direct_async.di]++;
 		ipu_dc_use_count++;
 		break;
@@ -713,7 +737,7 @@ int32_t ipu_init_channel(ipu_channel_t channel, ipu_channel_params_t *params)
 		}
 
 		g_dc_di_assignment[9] = params->direct_async.di;
-		_ipu_dc_init(9, params->direct_async.di, false);
+		_ipu_dc_init(9, params->direct_async.di, false, IPU_PIX_FMT_GENERIC);
 		ipu_di_use_count[params->direct_async.di]++;
 		ipu_dc_use_count++;
 		break;
@@ -724,31 +748,8 @@ int32_t ipu_init_channel(ipu_channel_t channel, ipu_channel_params_t *params)
 
 	/* Enable IPU sub module */
 	g_channel_init_mask |= 1L << IPU_CHAN_ID(channel);
-	if (ipu_ic_use_count == 1)
-		ipu_conf |= IPU_CONF_IC_EN;
-	if (ipu_vdi_use_count == 1) {
-		ipu_conf |= IPU_CONF_VDI_EN;
-		ipu_conf |= IPU_CONF_IC_INPUT;
-	}
-	if (ipu_rot_use_count == 1)
-		ipu_conf |= IPU_CONF_ROT_EN;
-	if (ipu_dc_use_count == 1)
-		ipu_conf |= IPU_CONF_DC_EN;
-	if (ipu_dp_use_count == 1)
-		ipu_conf |= IPU_CONF_DP_EN;
-	if (ipu_dmfc_use_count == 1)
-		ipu_conf |= IPU_CONF_DMFC_EN;
-	if (ipu_di_use_count[0] == 1) {
-		ipu_conf |= IPU_CONF_DI0_EN;
-	}
-	if (ipu_di_use_count[1] == 1) {
-		ipu_conf |= IPU_CONF_DI1_EN;
-	}
-	if (ipu_smfc_use_count == 1)
-		ipu_conf |= IPU_CONF_SMFC_EN;
 
 	__raw_writel(ipu_conf, IPU_CONF);
-
 err:
 	spin_unlock_irqrestore(&ipu_lock, lock_flags);
 	return ret;
@@ -775,8 +776,8 @@ void ipu_uninit_channel(ipu_channel_t channel)
 
 	/* Make sure channel is disabled */
 	/* Get input and output dma channels */
-	in_dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
-	out_dma = channel_2_dma(channel, IPU_VIDEO_IN_BUFFER);
+	in_dma = channel_2_dma(channel, IPU_VIDEO_IN_BUFFER);
+	out_dma = channel_2_dma(channel, IPU_OUTPUT_BUFFER);
 
 	if (idma_is_set(IDMAC_CHA_EN, in_dma) ||
 	    idma_is_set(IDMAC_CHA_EN, out_dma)) {
@@ -796,8 +797,10 @@ void ipu_uninit_channel(ipu_channel_t channel)
 	reg = __raw_readl(IPU_CHA_DB_MODE_SEL(out_dma));
 	__raw_writel(reg & ~idma_mask(out_dma), IPU_CHA_DB_MODE_SEL(out_dma));
 
-	g_sec_chan_en[IPU_CHAN_ID(channel)] = false;
-	g_thrd_chan_en[IPU_CHAN_ID(channel)] = false;
+	if (_ipu_is_ic_chan(in_dma) || _ipu_is_dp_graphic_chan(in_dma)) {
+		g_sec_chan_en[IPU_CHAN_ID(channel)] = false;
+		g_thrd_chan_en[IPU_CHAN_ID(channel)] = false;
+	}
 
 	switch (channel) {
 	case CSI_MEM0:
@@ -849,6 +852,9 @@ void ipu_uninit_channel(ipu_channel_t channel)
 		reg = __raw_readl(IPU_FS_PROC_FLOW1);
 		__raw_writel(reg & ~FS_VF_IN_VALID, IPU_FS_PROC_FLOW1);
 		break;
+	case MEM_VDI_PRP_VF_MEM_P:
+	case MEM_VDI_PRP_VF_MEM_N:
+		break;
 	case MEM_ROT_VF_MEM:
 		ipu_rot_use_count--;
 		ipu_ic_use_count--;
@@ -913,6 +919,7 @@ void ipu_uninit_channel(ipu_channel_t channel)
 	if (ipu_ic_use_count == 0)
 		ipu_conf &= ~IPU_CONF_IC_EN;
 	if (ipu_vdi_use_count == 0) {
+		ipu_conf &= ~IPU_CONF_ISP_EN;
 		ipu_conf &= ~IPU_CONF_VDI_EN;
 		ipu_conf &= ~IPU_CONF_IC_INPUT;
 	}
@@ -1239,8 +1246,6 @@ int32_t ipu_select_buffer(ipu_channel_t channel, ipu_buffer_t type,
 		__raw_writel(idma_mask(dma_chan) | reg,
 			     IPU_CHA_BUF1_RDY(dma_chan));
 	}
-	if (channel == MEM_VDI_PRP_VF_MEM)
-		_ipu_vdi_toggle_top_field_man();
 	return 0;
 }
 EXPORT_SYMBOL(ipu_select_buffer);
@@ -1269,10 +1274,9 @@ int32_t ipu_select_multi_vdi_buffer(uint32_t bufNum)
 		__raw_writel(mask_bit | reg, IPU_CHA_BUF0_RDY(dma_chan));
 	} else {
 		/*Mark buffer 1 as ready. */
-		reg = __raw_readl(IPU_CHA_BUF0_RDY(dma_chan));
+		reg = __raw_readl(IPU_CHA_BUF1_RDY(dma_chan));
 		__raw_writel(mask_bit | reg, IPU_CHA_BUF1_RDY(dma_chan));
 	}
-	_ipu_vdi_toggle_top_field_man();
 	return 0;
 }
 EXPORT_SYMBOL(ipu_select_multi_vdi_buffer);
@@ -1282,7 +1286,7 @@ static int proc_dest_sel[] =
   { 0, 1, 1, 3, 5, 5, 4, 7, 8, 9, 10, 11, 12, 14, 15, 16,
     0, 1, 1, 5, 5, 5, 5, 5, 7, 8, 9, 10, 11, 12, 14, 31 };
 static int proc_src_sel[] = { 0, 6, 7, 6, 7, 8, 5, NA, NA, NA,
-  NA, NA, NA, NA, NA,  1,  2,  3,  4,  7,  8, NA, NA, NA };
+  NA, NA, NA, NA, NA,  1,  2,  3,  4,  7,  8, NA, 8, NA };
 static int disp_src_sel[] = { 0, 6, 7, 8, 3, 4, 5, NA, NA, NA,
   NA, NA, NA, NA, NA,  1, NA,  2, NA,  3,  4,  4,  4,  4 };
 
@@ -1659,6 +1663,7 @@ int32_t ipu_enable_channel(ipu_channel_t channel)
 {
 	uint32_t reg;
 	unsigned long lock_flags;
+	uint32_t ipu_conf;
 	uint32_t in_dma;
 	uint32_t out_dma;
 	uint32_t sec_dma;
@@ -1675,6 +1680,32 @@ int32_t ipu_enable_channel(ipu_channel_t channel)
 
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
+	ipu_conf = __raw_readl(IPU_CONF);
+	if (ipu_di_use_count[0] > 0) {
+		ipu_conf |= IPU_CONF_DI0_EN;
+	}
+	if (ipu_di_use_count[1] > 0) {
+		ipu_conf |= IPU_CONF_DI1_EN;
+	}
+	if (ipu_dp_use_count > 0)
+		ipu_conf |= IPU_CONF_DP_EN;
+	if (ipu_dc_use_count > 0)
+		ipu_conf |= IPU_CONF_DC_EN;
+	if (ipu_dmfc_use_count > 0)
+		ipu_conf |= IPU_CONF_DMFC_EN;
+	if (ipu_ic_use_count > 0)
+		ipu_conf |= IPU_CONF_IC_EN;
+	if (ipu_vdi_use_count > 0) {
+		ipu_conf |= IPU_CONF_ISP_EN;
+		ipu_conf |= IPU_CONF_VDI_EN;
+		ipu_conf |= IPU_CONF_IC_INPUT;
+	}
+	if (ipu_rot_use_count > 0)
+		ipu_conf |= IPU_CONF_ROT_EN;
+	if (ipu_smfc_use_count > 0)
+		ipu_conf |= IPU_CONF_SMFC_EN;
+	__raw_writel(ipu_conf, IPU_CONF);
+
 	if (idma_is_valid(in_dma)) {
 		reg = __raw_readl(IDMAC_CHA_EN(in_dma));
 		__raw_writel(reg | idma_mask(in_dma), IDMAC_CHA_EN(in_dma));
@@ -1710,8 +1741,11 @@ int32_t ipu_enable_channel(ipu_channel_t channel)
 	}
 
 	if ((channel == MEM_DC_SYNC) || (channel == MEM_BG_SYNC) ||
-	    (channel == MEM_FG_SYNC))
+	    (channel == MEM_FG_SYNC)) {
+		reg = __raw_readl(IDMAC_WM_EN(in_dma));
+		__raw_writel(reg | idma_mask(in_dma), IDMAC_WM_EN(in_dma));
 		_ipu_dp_dc_enable(channel);
+	}
 
 	if (_ipu_is_ic_chan(in_dma) || _ipu_is_ic_chan(out_dma) ||
 		_ipu_is_irt_chan(in_dma) || _ipu_is_irt_chan(out_dma))
@@ -1726,6 +1760,38 @@ int32_t ipu_enable_channel(ipu_channel_t channel)
 EXPORT_SYMBOL(ipu_enable_channel);
 
 /*!
+ * This function check buffer ready for a logical channel.
+ *
+ * @param       channel         Input parameter for the logical channel ID.
+ *
+ * @param       type            Input parameter which buffer to clear.
+ *
+ * @param       bufNum          Input parameter for which buffer number clear
+ * 				ready state.
+ *
+ */
+int32_t ipu_check_buffer_busy(ipu_channel_t channel, ipu_buffer_t type,
+		uint32_t bufNum)
+{
+	uint32_t dma_chan = channel_2_dma(channel, type);
+	uint32_t reg;
+
+	if (dma_chan == IDMA_CHAN_INVALID)
+		return -EINVAL;
+
+	if (bufNum == 0)
+		reg = __raw_readl(IPU_CHA_BUF0_RDY(dma_chan));
+	else
+		reg = __raw_readl(IPU_CHA_BUF1_RDY(dma_chan));
+
+	if (reg & idma_mask(dma_chan))
+		return 1;
+	else
+		return 0;
+}
+EXPORT_SYMBOL(ipu_check_buffer_busy);
+
+/*!
  * This function clear buffer ready for a logical channel.
  *
  * @param       channel         Input parameter for the logical channel ID.
@@ -1817,7 +1883,30 @@ int32_t ipu_disable_channel(ipu_channel_t channel, bool wait_for_stop)
 
 	if ((channel == MEM_BG_SYNC) || (channel == MEM_FG_SYNC) ||
 	    (channel == MEM_DC_SYNC)) {
+		if (channel == MEM_FG_SYNC)
+			ipu_disp_set_window_pos(channel, 0, 0);
+
 		_ipu_dp_dc_disable(channel, false);
+
+		/*
+		 * wait for BG channel EOF then disable FG-IDMAC,
+		 * it avoid FG NFB4EOF error.
+		 */
+		if (channel == MEM_FG_SYNC) {
+			int timeout = 50;
+
+			__raw_writel(IPUIRQ_2_MASK(IPU_IRQ_BG_SYNC_EOF),
+					IPUIRQ_2_STATREG(IPU_IRQ_BG_SYNC_EOF));
+			while ((__raw_readl(IPUIRQ_2_STATREG(IPU_IRQ_BG_SYNC_EOF)) &
+					IPUIRQ_2_MASK(IPU_IRQ_BG_SYNC_EOF)) == 0) {
+				msleep(10);
+				timeout -= 10;
+				if (timeout <= 0) {
+					dev_err(g_ipu_dev, "warning: wait for bg sync eof timeout\n");
+					break;
+				}
+			}
+		}
 	} else if (wait_for_stop) {
 		while (idma_is_set(IDMAC_CHA_BUSY, in_dma) ||
 		       idma_is_set(IDMAC_CHA_BUSY, out_dma) ||
@@ -1861,6 +1950,12 @@ int32_t ipu_disable_channel(ipu_channel_t channel, bool wait_for_stop)
 
 	spin_lock_irqsave(&ipu_lock, lock_flags);
 
+	if ((channel == MEM_BG_SYNC) || (channel == MEM_FG_SYNC) ||
+	    (channel == MEM_DC_SYNC)) {
+		reg = __raw_readl(IDMAC_WM_EN(in_dma));
+		__raw_writel(reg & ~idma_mask(in_dma), IDMAC_WM_EN(in_dma));
+	}
+
 	/* Disable IC task */
 	if (_ipu_is_ic_chan(in_dma) || _ipu_is_ic_chan(out_dma) ||
 		_ipu_is_irt_chan(in_dma) || _ipu_is_irt_chan(out_dma))
@@ -1897,8 +1992,6 @@ int32_t ipu_disable_channel(ipu_channel_t channel, bool wait_for_stop)
 
 	g_channel_enable_mask &= ~(1L << IPU_CHAN_ID(channel));
 
-	spin_unlock_irqrestore(&ipu_lock, lock_flags);
-
 	/* Set channel buffers NOT to be ready */
 	if (idma_is_valid(in_dma)) {
 		ipu_clear_buffer_ready(channel, IPU_VIDEO_IN_BUFFER, 0);
@@ -1917,6 +2010,8 @@ int32_t ipu_disable_channel(ipu_channel_t channel, bool wait_for_stop)
 		ipu_clear_buffer_ready(channel, IPU_ALPHA_IN_BUFFER, 1);
 	}
 
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+
 	return 0;
 }
 EXPORT_SYMBOL(ipu_disable_channel);
@@ -2029,7 +2124,6 @@ static irqreturn_t ipu_irq_handler(int irq, void *desc)
 						       dev_id);
 		}
 	}
-
 	return result;
 }
 
diff --git a/drivers/mxc/ipu3/ipu_device.c b/drivers/mxc/ipu3/ipu_device.c
index 27455fe26ab6..bf71ea833f58 100644
--- a/drivers/mxc/ipu3/ipu_device.c
+++ b/drivers/mxc/ipu3/ipu_device.c
@@ -124,7 +124,6 @@ static int mxc_ipu_ioctl(struct inode *inode, struct file *file,
 				(&parm, (ipu_channel_buf_parm *) arg,
 				sizeof(ipu_channel_buf_parm)))
 				return -EFAULT;
-
 			ret =
 				ipu_init_channel_buffer(
 						parm.channel, parm.type,
@@ -183,6 +182,17 @@ static int mxc_ipu_ioctl(struct inode *inode, struct file *file,
 
 		}
 		break;
+	case IPU_SELECT_MULTI_VDI_BUFFER:
+		{
+			uint32_t parm;
+			if (copy_from_user
+				(&parm, (uint32_t *) arg,
+				sizeof(uint32_t)))
+				return -EFAULT;
+
+			ret = ipu_select_multi_vdi_buffer(parm);
+		}
+		break;
 	case IPU_LINK_CHANNELS:
 		{
 			ipu_channel_link link;
@@ -225,7 +235,6 @@ static int mxc_ipu_ioctl(struct inode *inode, struct file *file,
 				(&info, (ipu_channel_info *) arg,
 				 sizeof(ipu_channel_info)))
 				return -EFAULT;
-
 			ret = ipu_disable_channel(info.channel,
 				info.stop);
 		}
@@ -435,7 +444,7 @@ static int mxc_ipu_ioctl(struct inode *inode, struct file *file,
 
 static int mxc_ipu_mmap(struct file *file, struct vm_area_struct *vma)
 {
-//	vma->vm_page_prot = pgprot_writethru(vma->vm_page_prot);
+	vma->vm_page_prot = pgprot_writethru(vma->vm_page_prot);
 
 	if (remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
 				vma->vm_end - vma->vm_start,
@@ -452,12 +461,20 @@ static int mxc_ipu_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
+int mxc_ipu_fsync(struct file *filp, struct dentry *dentry, int datasync)
+{
+	flush_cache_all();
+	outer_flush_all();
+	return 0;
+}
+
 static struct file_operations mxc_ipu_fops = {
 	.owner = THIS_MODULE,
 	.open = mxc_ipu_open,
 	.mmap = mxc_ipu_mmap,
 	.release = mxc_ipu_release,
-	.ioctl = mxc_ipu_ioctl
+	.ioctl = mxc_ipu_ioctl,
+	.fsync = mxc_ipu_fsync
 };
 
 int register_ipu_device()
diff --git a/drivers/mxc/ipu3/ipu_disp.c b/drivers/mxc/ipu3/ipu_disp.c
index 00c5008d149f..14dde404990b 100644
--- a/drivers/mxc/ipu3/ipu_disp.c
+++ b/drivers/mxc/ipu3/ipu_disp.c
@@ -147,6 +147,37 @@ static int __init dmfc_setup(char *options)
 }
 __setup("dmfc=", dmfc_setup);
 
+static bool _ipu_update_dmfc_used_size(int dma_chan, int width, int dmfc_size)
+{
+	u32 fifo_size_5f = 1;
+	u32 dmfc_dp_chan = __raw_readl(DMFC_DP_CHAN);
+
+	if ((width > 352) && (dmfc_size == (256 * 4)))
+		fifo_size_5f = 1;
+	else if (width > 176)
+		fifo_size_5f = 2;
+	else if (width > 88)
+		fifo_size_5f = 3;
+	else if (width > 44)
+		fifo_size_5f = 4;
+	else if (width > 22)
+		fifo_size_5f = 5;
+	else if (width > 11)
+		fifo_size_5f = 6;
+	else if (width > 6)
+		fifo_size_5f = 7;
+	else
+		return false;
+
+	if (dma_chan == 27) {
+		dmfc_dp_chan &= ~DMFC_FIFO_SIZE_5F;
+		dmfc_dp_chan |= fifo_size_5f << 11;
+		__raw_writel(dmfc_dp_chan, DMFC_DP_CHAN);
+	}
+
+	return true;
+}
+
 void _ipu_dmfc_set_wait4eot(int dma_chan, int width)
 {
 	u32 dmfc_gen1 = __raw_readl(DMFC_GENERAL1);
@@ -169,7 +200,7 @@ void _ipu_dmfc_set_wait4eot(int dma_chan, int width)
 		else
 			dmfc_gen1 &= ~(1UL << 22);
 	} else if (dma_chan == 27) { /*5F*/
-		if (dmfc_size_27/width > 2)
+		if (!_ipu_update_dmfc_used_size(dma_chan, width, dmfc_size_27))
 			dmfc_gen1 |= 1UL << 21;
 		else
 			dmfc_gen1 &= ~(1UL << 21);
@@ -245,6 +276,24 @@ static void _ipu_di_sync_config(int di, int wave_gen,
 	__raw_writel(reg, DI_STP_REP(di, wave_gen));
 }
 
+static void _ipu_dc_map_link(int current_map,
+			     int base_map_0, int buf_num_0,
+			     int base_map_1, int buf_num_1,
+			     int base_map_2, int buf_num_2)
+{
+	int ptr_0 = base_map_0 * 3 + buf_num_0;
+	int ptr_1 = base_map_1 * 3 + buf_num_1;
+	int ptr_2 = base_map_2 * 3 + buf_num_2;
+	int ptr;
+	u32 reg;
+	ptr = (ptr_2 << 10) +  (ptr_1 << 5) + ptr_0;
+
+	reg = __raw_readl(DC_MAP_CONF_PTR(current_map));
+	reg &= ~(0x1F << ((16 * (current_map & 0x1))));
+	reg |= ptr << ((16 * (current_map & 0x1)));
+	__raw_writel(reg, DC_MAP_CONF_PTR(current_map));
+}
+
 static void _ipu_dc_map_config(int map, int byte_num, int offset, int mask)
 {
 	int ptr = map * 3 + byte_num;
@@ -269,32 +318,63 @@ static void _ipu_dc_map_clear(int map)
 }
 
 static void _ipu_dc_write_tmpl(int word, u32 opcode, u32 operand, int map,
-			       int wave, int glue, int sync)
+			       int wave, int glue, int sync, int stop)
 {
 	u32 reg;
-	int stop = 1;
-
-	reg = sync;
-	reg |= (glue << 4);
-	reg |= (++wave << 11);
-	reg |= (++map << 15);
-	reg |= (operand << 20) & 0xFFF00000;
-	__raw_writel(reg, ipu_dc_tmpl_reg + word * 2);
-
-	reg = (operand >> 12);
-	reg |= opcode << 4;
-	reg |= (stop << 9);
-	__raw_writel(reg, ipu_dc_tmpl_reg + word * 2 + 1);
+
+	if (opcode == WRG) {
+		reg = sync;
+		reg |= (glue << 4);
+		reg |= (++wave << 11);
+		reg |= ((operand & 0x1FFFF) << 15);
+		__raw_writel(reg, ipu_dc_tmpl_reg + word * 2);
+
+		reg = (operand >> 17);
+		reg |= opcode << 7;
+		reg |= (stop << 9);
+		__raw_writel(reg, ipu_dc_tmpl_reg + word * 2 + 1);
+	} else {
+		reg = sync;
+		reg |= (glue << 4);
+		reg |= (++wave << 11);
+		reg |= (++map << 15);
+		reg |= (operand << 20) & 0xFFF00000;
+		__raw_writel(reg, ipu_dc_tmpl_reg + word * 2);
+
+		reg = (operand >> 12);
+		reg |= opcode << 4;
+		reg |= (stop << 9);
+		__raw_writel(reg, ipu_dc_tmpl_reg + word * 2 + 1);
+	}
 }
 
 static void _ipu_dc_link_event(int chan, int event, int addr, int priority)
 {
 	u32 reg;
-
-	reg = __raw_readl(DC_RL_CH(chan, event));
-	reg &= ~(0xFFFF << (16 * (event & 0x1)));
-	reg |= ((addr << 8) | priority) << (16 * (event & 0x1));
-	__raw_writel(reg, DC_RL_CH(chan, event));
+	u32 address_shift;
+	if (event < DC_EVEN_UGDE0) {
+		reg = __raw_readl(DC_RL_CH(chan, event));
+		reg &= ~(0xFFFF << (16 * (event & 0x1)));
+		reg |= ((addr << 8) | priority) << (16 * (event & 0x1));
+		__raw_writel(reg, DC_RL_CH(chan, event));
+	} else {
+		reg = __raw_readl(DC_UGDE_0((event - DC_EVEN_UGDE0) / 2));
+		if ((event - DC_EVEN_UGDE0) & 0x1) {
+			reg &= ~(0x2FF << 16);
+			reg |= (addr << 16);
+			reg |= priority ? (2 << 24) : 0x0;
+		} else {
+			reg &= ~0xFC00FFFF;
+			if (priority)
+				chan = (chan >> 1) +
+					((((chan & 0x1) + ((chan & 0x2) >> 1))) | (chan >> 3));
+			else
+				chan = 0x7;
+			address_shift = ((event - DC_EVEN_UGDE0) >> 1) ? 7 : 8;
+			reg |= (addr << address_shift) | (priority << 3) | chan;
+		}
+		__raw_writel(reg, DC_UGDE_0((event - DC_EVEN_UGDE0) / 2));
+	}
 }
 
 /*     Y = R *  1.200 + G *  2.343 + B *  .453 + 0.250;
@@ -503,7 +583,7 @@ void _ipu_dp_uninit(ipu_channel_t channel)
 	__ipu_dp_csc_setup(dp, dp_csc_array[bg_csc_type][fg_csc_type], false);
 }
 
-void _ipu_dc_init(int dc_chan, int di, bool interlaced)
+void _ipu_dc_init(int dc_chan, int di, bool interlaced, uint32_t pixel_fmt)
 {
 	u32 reg = 0;
 
@@ -517,10 +597,24 @@ void _ipu_dc_init(int dc_chan, int di, bool interlaced)
 				_ipu_dc_link_event(dc_chan, DC_EVT_NL, 2, 3);
 				_ipu_dc_link_event(dc_chan, DC_EVT_EOL, 3, 2);
 				_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA, 4, 1);
+				if ((pixel_fmt == IPU_PIX_FMT_YUYV) ||
+				(pixel_fmt == IPU_PIX_FMT_UYVY) ||
+				(pixel_fmt == IPU_PIX_FMT_YVYU) ||
+				(pixel_fmt == IPU_PIX_FMT_VYUY)) {
+					_ipu_dc_link_event(dc_chan, DC_ODD_UGDE1, 9, 5);
+					_ipu_dc_link_event(dc_chan, DC_EVEN_UGDE1, 8, 5);
+				}
 			} else {
 				_ipu_dc_link_event(dc_chan, DC_EVT_NL, 5, 3);
 				_ipu_dc_link_event(dc_chan, DC_EVT_EOL, 6, 2);
 				_ipu_dc_link_event(dc_chan, DC_EVT_NEW_DATA, 7, 1);
+				if ((pixel_fmt == IPU_PIX_FMT_YUYV) ||
+				(pixel_fmt == IPU_PIX_FMT_UYVY) ||
+				(pixel_fmt == IPU_PIX_FMT_YVYU) ||
+				(pixel_fmt == IPU_PIX_FMT_VYUY)) {
+					_ipu_dc_link_event(dc_chan, DC_ODD_UGDE0, 10, 5);
+					_ipu_dc_link_event(dc_chan, DC_EVEN_UGDE0, 11, 5);
+				}
 			}
 		}
 		_ipu_dc_link_event(dc_chan, DC_EVT_NF, 0, 0);
@@ -562,6 +656,10 @@ void _ipu_dc_uninit(int dc_chan)
 		_ipu_dc_link_event(dc_chan, DC_EVT_EOFIELD, 0, 0);
 		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_CHAN, 0, 0);
 		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_ADDR, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_ODD_UGDE0, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVEN_UGDE0, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_ODD_UGDE1, 0, 0);
+		_ipu_dc_link_event(dc_chan, DC_EVEN_UGDE1, 0, 0);
 	} else if ((dc_chan == 8) || (dc_chan == 9)) {
 		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_ADDR_W_0, 0, 0);
 		_ipu_dc_link_event(dc_chan, DC_EVT_NEW_ADDR_W_1, 0, 0);
@@ -637,6 +735,26 @@ static bool dc_swap;
 static irqreturn_t dc_irq_handler(int irq, void *dev_id)
 {
 	struct completion *comp = dev_id;
+	uint32_t reg;
+	uint32_t dc_chan;
+
+	if (irq == IPU_IRQ_DC_FC_1)
+		dc_chan = 1;
+	else
+		dc_chan = 5;
+
+	if (!dc_swap) {
+		reg = __raw_readl(DC_WR_CH_CONF(dc_chan));
+		reg &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
+		__raw_writel(reg, DC_WR_CH_CONF(dc_chan));
+
+		reg = __raw_readl(IPU_DISP_GEN);
+		if (g_dc_di_assignment[dc_chan])
+			reg &= ~DI1_COUNTER_RELEASE;
+		else
+			reg &= ~DI0_COUNTER_RELEASE;
+		__raw_writel(reg, IPU_DISP_GEN);
+	}
 
 	complete(comp);
 	return IRQ_HANDLED;
@@ -689,29 +807,6 @@ void _ipu_dp_dc_disable(ipu_channel_t channel, bool swap)
 			if (timeout <= 0)
 				break;
 		}
-
-		timeout = 50;
-
-		/*
-		 * Wait for DC triple buffer to empty,
-		 * this check is useful for tv overlay.
-		 */
-		if (g_dc_di_assignment[dc_chan] == 0)
-			while ((__raw_readl(DC_STAT) & 0x00000002)
-			       != 0x00000002) {
-				msleep(2);
-				timeout -= 2;
-				if (timeout <= 0)
-					break;
-			}
-		else if (g_dc_di_assignment[dc_chan] == 1)
-			while ((__raw_readl(DC_STAT) & 0x00000020)
-			       != 0x00000020) {
-				msleep(2);
-				timeout -= 2;
-				if (timeout <= 0)
-					break;
-			}
 		return;
 	} else {
 		return;
@@ -743,39 +838,6 @@ void _ipu_dp_dc_disable(ipu_channel_t channel, bool swap)
 		__raw_writel(reg, DC_WR_CH_CONF(dc_chan));
 		spin_unlock_irqrestore(&ipu_lock, lock_flags);
 	} else {
-		timeout = 50;
-
-		/* Wait for DC triple buffer to empty */
-		if (g_dc_di_assignment[dc_chan] == 0)
-			while ((__raw_readl(DC_STAT) & 0x00000002)
-				!= 0x00000002) {
-				msleep(2);
-				timeout -= 2;
-				if (timeout <= 0)
-					break;
-			}
-		else if (g_dc_di_assignment[dc_chan] == 1)
-			while ((__raw_readl(DC_STAT) & 0x00000020)
-				!= 0x00000020) {
-				msleep(2);
-				timeout -= 2;
-				if (timeout <= 0)
-					break;
-			}
-
-		spin_lock_irqsave(&ipu_lock, lock_flags);
-		reg = __raw_readl(DC_WR_CH_CONF(dc_chan));
-		reg &= ~DC_WR_CH_CONF_PROG_TYPE_MASK;
-		__raw_writel(reg, DC_WR_CH_CONF(dc_chan));
-
-		reg = __raw_readl(IPU_DISP_GEN);
-		if (g_dc_di_assignment[dc_chan])
-			reg &= ~DI1_COUNTER_RELEASE;
-		else
-			reg &= ~DI0_COUNTER_RELEASE;
-		__raw_writel(reg, IPU_DISP_GEN);
-
-		spin_unlock_irqrestore(&ipu_lock, lock_flags);
 		/* Clock is already off because it must be done quickly, but
 		   we need to fix the ref count */
 		clk_disable(g_pixel_clk[g_dc_di_assignment[dc_chan]]);
@@ -821,6 +883,34 @@ void _ipu_init_dc_mappings(void)
 	_ipu_dc_map_config(4, 0, 5, 0xFC);
 	_ipu_dc_map_config(4, 1, 13, 0xFC);
 	_ipu_dc_map_config(4, 2, 21, 0xFC);
+
+	/* IPU_PIX_FMT_VYUY 16bit width */
+	_ipu_dc_map_clear(5);
+	_ipu_dc_map_config(5, 0, 7, 0xFF);
+	_ipu_dc_map_config(5, 1, 0, 0x0);
+	_ipu_dc_map_config(5, 2, 15, 0xFF);
+	_ipu_dc_map_clear(6);
+	_ipu_dc_map_config(6, 0, 0, 0x0);
+	_ipu_dc_map_config(6, 1, 7, 0xFF);
+	_ipu_dc_map_config(6, 2, 15, 0xFF);
+
+	/* IPU_PIX_FMT_UYUV 16bit width */
+	_ipu_dc_map_clear(7);
+	_ipu_dc_map_link(7, 6, 0, 6, 1, 6, 2);
+	_ipu_dc_map_clear(8);
+	_ipu_dc_map_link(8, 5, 0, 5, 1, 5, 2);
+
+	/* IPU_PIX_FMT_YUYV 16bit width */
+	_ipu_dc_map_clear(9);
+	_ipu_dc_map_link(9, 5, 2, 5, 1, 5, 0);
+	_ipu_dc_map_clear(10);
+	_ipu_dc_map_link(10, 5, 1, 5, 2, 5, 0);
+
+	/* IPU_PIX_FMT_YVYU 16bit width */
+	_ipu_dc_map_clear(11);
+	_ipu_dc_map_link(11, 5, 1, 5, 2, 5, 0);
+	_ipu_dc_map_clear(12);
+	_ipu_dc_map_link(12, 5, 2, 5, 1, 5, 0);
 }
 
 int _ipu_pixfmt_to_map(uint32_t fmt)
@@ -837,6 +927,14 @@ int _ipu_pixfmt_to_map(uint32_t fmt)
 		return 3;
 	case IPU_PIX_FMT_LVDS666:
 		return 4;
+	case IPU_PIX_FMT_VYUY:
+		return 6;
+	case IPU_PIX_FMT_UYVY:
+		return 8;
+	case IPU_PIX_FMT_YUYV:
+		return 10;
+	case IPU_PIX_FMT_YVYU:
+		return 12;
 	}
 
 	return -1;
@@ -964,22 +1062,22 @@ int32_t ipu_init_sync_panel(int disp, uint32_t pixel_clk,
 	dev_dbg(g_ipu_dev, "pixel clk = %d\n", pixel_clk);
 
 	if (sig.ext_clk) {
-		/* Set the  PLL to be an even multiple of the pixel clock. not round div for tvout*/
-		if ((clk_get_usecount(g_pixel_clk[0]) == 0) &&
-				(clk_get_usecount(g_pixel_clk[1]) == 0)) {
+		/*
+		 * Set the  PLL to be an even multiple of the pixel clock.
+		 * Not round div for tvout and ldb.
+		 * Did not consider both DI come from the same ext clk, if
+		 * meet such case, ext clk rate should be set specially.
+		 */
+		if (clk_get_usecount(g_pixel_clk[disp]) == 0) {
 			di_parent = clk_get_parent(g_di_clk[disp]);
-			if (strcmp(di_parent->name, "tve_clk") != 0) {
-				rounded_pixel_clk =
-					clk_round_rate(g_pixel_clk[disp], pixel_clk);
-				div  = clk_get_rate(di_parent) / rounded_pixel_clk;
-				if (div % 2)
-					div++;
-
-				if (clk_get_rate(di_parent) != div * rounded_pixel_clk)
-					clk_set_rate(di_parent, div * rounded_pixel_clk);
-				msleep(10);
-				clk_set_rate(g_di_clk[disp], 2 * rounded_pixel_clk);
-				msleep(10);
+			if (strcmp(di_parent->name, "tve_clk") != 0 &&
+			    strcmp(di_parent->name, "ldb_di0_clk") != 0 &&
+			    strcmp(di_parent->name, "ldb_di1_clk") != 0)  {
+				rounded_pixel_clk = pixel_clk * 2;
+				while (rounded_pixel_clk < 150000000)
+					rounded_pixel_clk += pixel_clk * 2;
+				clk_set_rate(di_parent, rounded_pixel_clk);
+				clk_set_rate(g_di_clk[disp], pixel_clk);
 			}
 		}
 		clk_set_parent(g_pixel_clk[disp], g_di_clk[disp]);
@@ -1019,7 +1117,7 @@ int32_t ipu_init_sync_panel(int disp, uint32_t pixel_clk,
 	di_gen = __raw_readl(DI_GENERAL(disp));
 
 	if (sig.interlaced) {
-		if (cpu_is_mx51_rev(CHIP_REV_2_0)) {
+		if (g_ipu_hw_rev >= 2) {
 			/* Setup internal HSYNC waveform */
 			_ipu_di_sync_config(
 					disp, 			/* display */
@@ -1259,7 +1357,7 @@ int32_t ipu_init_sync_panel(int disp, uint32_t pixel_clk,
 		}
 
 		/* Init template microcode */
-		_ipu_dc_write_tmpl(0, WROD(0), 0, map, SYNC_WAVE, 0, 8);
+		_ipu_dc_write_tmpl(0, WROD(0), 0, map, SYNC_WAVE, 0, 8, 1);
 
 		if (sig.Hsync_pol)
 			di_gen |= DI_GEN_POLARITY_3;
@@ -1322,13 +1420,31 @@ int32_t ipu_init_sync_panel(int disp, uint32_t pixel_clk,
 
 		/* Init template microcode */
 		if (disp) {
-		   _ipu_dc_write_tmpl(2, WROD(0), 0, map, SYNC_WAVE, 8, 5);
-		   _ipu_dc_write_tmpl(3, WROD(0), 0, map, SYNC_WAVE, 4, 5);
-		   _ipu_dc_write_tmpl(4, WROD(0), 0, map, SYNC_WAVE, 0, 5);
+			if ((pixel_fmt == IPU_PIX_FMT_YUYV) ||
+				(pixel_fmt == IPU_PIX_FMT_UYVY) ||
+				(pixel_fmt == IPU_PIX_FMT_YVYU) ||
+				(pixel_fmt == IPU_PIX_FMT_VYUY)) {
+				_ipu_dc_write_tmpl(8, WROD(0), 0, (map - 1), SYNC_WAVE, 0, 5, 1);
+				_ipu_dc_write_tmpl(9, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
+				/* configure user events according to DISP NUM */
+				__raw_writel((width - 1), DC_UGDE_3(disp));
+			}
+		   _ipu_dc_write_tmpl(2, WROD(0), 0, map, SYNC_WAVE, 8, 5, 1);
+		   _ipu_dc_write_tmpl(3, WRG, 0, map, SYNC_WAVE, 4, 5, 1);
+		   _ipu_dc_write_tmpl(4, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
 		} else {
-		   _ipu_dc_write_tmpl(5, WROD(0), 0, map, SYNC_WAVE, 8, 5);
-		   _ipu_dc_write_tmpl(6, WROD(0), 0, map, SYNC_WAVE, 4, 5);
-		   _ipu_dc_write_tmpl(7, WROD(0), 0, map, SYNC_WAVE, 0, 5);
+			if ((pixel_fmt == IPU_PIX_FMT_YUYV) ||
+				(pixel_fmt == IPU_PIX_FMT_UYVY) ||
+				(pixel_fmt == IPU_PIX_FMT_YVYU) ||
+				(pixel_fmt == IPU_PIX_FMT_VYUY)) {
+				_ipu_dc_write_tmpl(10, WROD(0), 0, (map - 1), SYNC_WAVE, 0, 5, 1);
+				_ipu_dc_write_tmpl(11, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
+				/* configure user events according to DISP NUM */
+				__raw_writel(width - 1, DC_UGDE_3(disp));
+			}
+		   _ipu_dc_write_tmpl(5, WROD(0), 0, map, SYNC_WAVE, 8, 5, 1);
+		   _ipu_dc_write_tmpl(6, WRG, 0, map, SYNC_WAVE, 4, 5, 1);
+		   _ipu_dc_write_tmpl(7, WROD(0), 0, map, SYNC_WAVE, 0, 5, 1);
 		}
 
 		if (sig.Hsync_pol)
@@ -1340,6 +1456,19 @@ int32_t ipu_init_sync_panel(int disp, uint32_t pixel_clk,
 			/* Set the clock to stop at counter 6. */
 			di_gen |= 0x6000000;
 	}
+	/* changinc DISP_CLK polarity: it can be wrong for some applications */
+	if ((pixel_fmt == IPU_PIX_FMT_YUYV) ||
+		(pixel_fmt == IPU_PIX_FMT_UYVY) ||
+		(pixel_fmt == IPU_PIX_FMT_YVYU) ||
+		(pixel_fmt == IPU_PIX_FMT_VYUY))
+			di_gen |= 0x00020000;
+	else {
+		/* Configure accordingly to the received configuration */
+		if (sig.clk_pol)
+			di_gen |= 0x00020000;
+		else
+			di_gen &= ~0x00020000;
+	}
 
 	__raw_writel(di_gen, DI_GENERAL(disp));
 
@@ -1402,7 +1531,7 @@ int ipu_init_async_panel(int disp, int type, uint32_t cycle_time,
 		_ipu_di_data_pin_config(disp, ASYNC_SER_WAVE, DI_PIN_SER_RS,
 					2, 0, 0);
 
-		_ipu_dc_write_tmpl(0x64, WROD(0), 0, map, ASYNC_SER_WAVE, 0, 0);
+		_ipu_dc_write_tmpl(0x64, WROD(0), 0, map, ASYNC_SER_WAVE, 0, 0, 1);
 
 		/* Configure DC for serial panel */
 		__raw_writel(0x14, DC_DISP_CONF1(DC_DISP_ID_SERIAL));
@@ -1679,6 +1808,39 @@ int32_t ipu_disp_set_window_pos(ipu_channel_t channel, int16_t x_pos,
 }
 EXPORT_SYMBOL(ipu_disp_set_window_pos);
 
+int32_t ipu_disp_get_window_pos(ipu_channel_t channel, int16_t *x_pos,
+				int16_t *y_pos)
+{
+	u32 reg;
+	unsigned long lock_flags;
+	uint32_t flow = 0;
+
+	if (channel == MEM_FG_SYNC)
+		flow = DP_SYNC;
+	else if (channel == MEM_FG_ASYNC0)
+		flow = DP_ASYNC0;
+	else if (channel == MEM_FG_ASYNC1)
+		flow = DP_ASYNC1;
+	else
+		return -EINVAL;
+
+	if (!g_ipu_clk_enabled)
+		clk_enable(g_ipu_clk);
+	spin_lock_irqsave(&ipu_lock, lock_flags);
+
+	reg = __raw_readl(DP_FG_POS(flow));
+
+	*x_pos = (reg >> 16) & 0x7FF;
+	*y_pos = reg & 0x7FF;
+
+	spin_unlock_irqrestore(&ipu_lock, lock_flags);
+	if (!g_ipu_clk_enabled)
+		clk_disable(g_ipu_clk);
+
+	return 0;
+}
+EXPORT_SYMBOL(ipu_disp_get_window_pos);
+
 void ipu_disp_direct_write(ipu_channel_t channel, u32 value, u32 offset)
 {
 	if (channel == DIRECT_ASYNC0)
diff --git a/drivers/mxc/ipu3/ipu_ic.c b/drivers/mxc/ipu3/ipu_ic.c
index 564fab0b699a..78c3a9228941 100644
--- a/drivers/mxc/ipu3/ipu_ic.c
+++ b/drivers/mxc/ipu3/ipu_ic.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2005-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -157,7 +157,6 @@ void _ipu_vdi_init(ipu_channel_t channel, ipu_channel_params_t *params)
 {
 	uint32_t reg;
 	uint32_t pixel_fmt;
-	bool top_field_0;
 
 	reg = ((params->mem_prp_vf_mem.in_height-1) << 16) |
 	  (params->mem_prp_vf_mem.in_width-1);
@@ -186,19 +185,7 @@ void _ipu_vdi_init(ipu_channel_t channel, ipu_channel_params_t *params)
 	}
 	__raw_writel(reg, VDI_C);
 
-	/* MED_MOTION and LOW_MOTION algorithm that are using 3 fields
-	 * should start presenting using the 2nd field.
-	 */
-	if (((params->mem_prp_vf_mem.field_fmt == V4L2_FIELD_INTERLACED_TB) &&
-	     (params->mem_prp_vf_mem.motion_sel != HIGH_MOTION)) ||
-	    ((params->mem_prp_vf_mem.field_fmt == V4L2_FIELD_INTERLACED_BT) &&
-	     (params->mem_prp_vf_mem.motion_sel == HIGH_MOTION)))
-		top_field_0 = false;
-	else
-		top_field_0 = true;
-
-	/* Buffer selection toggle the value therefore init val is inverted. */
-	_ipu_vdi_set_top_field_man(!top_field_0);
+	_ipu_vdi_set_top_field_man(false);
 
 	_ipu_vdi_set_motion(params->mem_prp_vf_mem.motion_sel);
 
@@ -227,13 +214,13 @@ void _ipu_ic_init_prpvf(ipu_channel_params_t *params, bool src_is_csi)
 
 	/* Setup horizontal resizing */
 	/* Upadeted for IC split case */
-	if (!(params->mem_prp_vf_mem.out_resize_ratio)) {
+	if (!(params->mem_prp_vf_mem.outh_resize_ratio)) {
 		_calc_resize_coeffs(params->mem_prp_vf_mem.in_width,
 				params->mem_prp_vf_mem.out_width,
 				&resizeCoeff, &downsizeCoeff);
 		reg |= (downsizeCoeff << 14) | resizeCoeff;
 	} else
-		reg |= params->mem_prp_vf_mem.out_resize_ratio;
+		reg |= params->mem_prp_vf_mem.outh_resize_ratio;
 
 	__raw_writel(reg, IC_PRP_VF_RSC);
 
@@ -349,13 +336,13 @@ void _ipu_ic_init_prpenc(ipu_channel_params_t *params, bool src_is_csi)
 
 	/* Setup horizontal resizing */
 	/* Upadeted for IC split case */
-	if (!(params->mem_prp_enc_mem.out_resize_ratio)) {
+	if (!(params->mem_prp_enc_mem.outh_resize_ratio)) {
 		_calc_resize_coeffs(params->mem_prp_enc_mem.in_width,
 				params->mem_prp_enc_mem.out_width,
 				&resizeCoeff, &downsizeCoeff);
 		reg |= (downsizeCoeff << 14) | resizeCoeff;
 	} else
-		reg |= params->mem_prp_enc_mem.out_resize_ratio;
+		reg |= params->mem_prp_enc_mem.outh_resize_ratio;
 
 	__raw_writel(reg, IC_PRP_ENC_RSC);
 
@@ -387,6 +374,8 @@ void _ipu_ic_init_prpenc(ipu_channel_params_t *params, bool src_is_csi)
 		ic_conf |= IC_CONF_RWS_EN;
 
 	__raw_writel(ic_conf, IC_CONF);
+
+//	ic_dump_register();
 }
 
 void _ipu_ic_uninit_prpenc(void)
@@ -418,20 +407,24 @@ void _ipu_ic_init_pp(ipu_channel_params_t *params)
 	ipu_color_space_t in_fmt, out_fmt;
 
 	/* Setup vertical resizing */
-	_calc_resize_coeffs(params->mem_pp_mem.in_height,
+	if (!(params->mem_pp_mem.outv_resize_ratio)) {
+		_calc_resize_coeffs(params->mem_pp_mem.in_height,
 			    params->mem_pp_mem.out_height,
 			    &resizeCoeff, &downsizeCoeff);
-	reg = (downsizeCoeff << 30) | (resizeCoeff << 16);
+		reg = (downsizeCoeff << 30) | (resizeCoeff << 16);
+	} else {
+		reg = (params->mem_pp_mem.outv_resize_ratio) << 16;
+	}
 
 	/* Setup horizontal resizing */
 	/* Upadeted for IC split case */
-	if (!(params->mem_pp_mem.out_resize_ratio)) {
+	if (!(params->mem_pp_mem.outh_resize_ratio)) {
 		_calc_resize_coeffs(params->mem_pp_mem.in_width,
 							params->mem_pp_mem.out_width,
 							&resizeCoeff, &downsizeCoeff);
 		reg |= (downsizeCoeff << 14) | resizeCoeff;
 	} else {
-		reg |= params->mem_pp_mem.out_resize_ratio;
+		reg |= params->mem_pp_mem.outh_resize_ratio;
 	}
 
 	__raw_writel(reg, IC_PP_RSC);
diff --git a/drivers/mxc/ipu3/ipu_param_mem.h b/drivers/mxc/ipu3/ipu_param_mem.h
index dab3b617db1c..30e6dc1005ba 100644
--- a/drivers/mxc/ipu3/ipu_param_mem.h
+++ b/drivers/mxc/ipu3/ipu_param_mem.h
@@ -155,8 +155,15 @@ static inline void _ipu_ch_param_init(int ch,
 		ipu_ch_param_set_field(&params, 1, 102, 14, stride - 1);
 	}
 
+	/* EBA is 8-byte aligned */
 	ipu_ch_param_set_field(&params, 1, 0, 29, addr0 >> 3);
 	ipu_ch_param_set_field(&params, 1, 29, 29, addr1 >> 3);
+	if (addr0%8)
+		dev_warn(g_ipu_dev,
+			 "IDMAC%d's EBA0 is not 8-byte aligned\n", ch);
+	if (addr1%8)
+		dev_warn(g_ipu_dev,
+			 "IDMAC%d's EBA1 is not 8-byte aligned\n", ch);
 
 	switch (pixel_fmt) {
 	case IPU_PIX_FMT_GENERIC:
@@ -210,13 +217,14 @@ static inline void _ipu_ch_param_init(int ch,
 	case IPU_PIX_FMT_ABGR32:
 		ipu_ch_param_set_field(&params, 0, 107, 3, 0);	/* bits/pixel */
 		ipu_ch_param_set_field(&params, 1, 85, 4, 7);	/* pix format */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 15);	/* burst size */
 
 		_ipu_ch_params_set_packing(&params, 8, 0, 8, 8, 8, 16, 8, 24);
 		break;
 	case IPU_PIX_FMT_UYVY:
 		ipu_ch_param_set_field(&params, 0, 107, 3, 3);	/* bits/pixel */
 		ipu_ch_param_set_field(&params, 1, 85, 4, 0xA);	/* pix format */
-		ipu_ch_param_set_field(&params, 1, 78, 7, 15);	/* burst size */
+		ipu_ch_param_set_field(&params, 1, 78, 7, 31);	/* burst size */
 		break;
 	case IPU_PIX_FMT_YUYV:
 		ipu_ch_param_set_field(&params, 0, 107, 3, 3);	/* bits/pixel */
@@ -289,13 +297,19 @@ static inline void _ipu_ch_param_init(int ch,
 		v_offset = v;
 	}
 
-	/* UBO and VBO are 22-bit */
+	/* UBO and VBO are 22-bit and 8-byte aligned */
 	if (u_offset/8 > 0x3fffff)
-		dev_err(g_ipu_dev,
-			"The value of U offset exceeds IPU limitation\n");
+		dev_warn(g_ipu_dev,
+			 "IDMAC%d's U offset exceeds IPU limitation\n", ch);
 	if (v_offset/8 > 0x3fffff)
-		dev_err(g_ipu_dev,
-			"The value of V offset exceeds IPU limitation\n");
+		dev_warn(g_ipu_dev,
+			 "IDMAC%d's V offset exceeds IPU limitation\n", ch);
+	if (u_offset%8)
+		dev_warn(g_ipu_dev,
+			 "IDMAC%d's U offset is not 8-byte aligned\n", ch);
+	if (v_offset%8)
+		dev_warn(g_ipu_dev,
+			 "IDMAC%d's V offset is not 8-byte aligned\n", ch);
 
 	ipu_ch_param_set_field(&params, 0, 46, 22, u_offset / 8);
 	ipu_ch_param_set_field(&params, 0, 68, 22, v_offset / 8);
@@ -386,6 +400,13 @@ static inline void _ipu_ch_param_set_interlaced_scan(uint32_t ch)
 	u32 stride;
 	ipu_ch_param_set_field(ipu_ch_param_addr(ch), 0, 113, 1, 1);
 	stride = ipu_ch_param_read_field(ipu_ch_param_addr(ch), 1, 102, 14) + 1;
+	/* ILO is 20-bit and 8-byte aligned */
+	if (stride/8 > 0xfffff)
+		dev_warn(g_ipu_dev,
+			 "IDMAC%d's ILO exceeds IPU limitation\n", ch);
+	if (stride%8)
+		dev_warn(g_ipu_dev,
+			 "IDMAC%d's ILO is not 8-byte aligned\n", ch);
 	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 58, 20, stride / 8);
 	stride *= 2;
 	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 1, 102, 14, stride - 1);
@@ -442,7 +463,7 @@ static inline void _ipu_ch_offset_update(int ch,
 					(uv_stride * vertical_offset / 2) +
 					horizontal_offset / 2;
 		v_offset = u_offset + (uv_stride * height / 2);
-		u_fix = u ? (u + (uv_stride * vertical_offset) +
+		u_fix = u ? (u + (uv_stride * vertical_offset / 2) +
 					(horizontal_offset / 2) -
 					(stride * vertical_offset) - (horizontal_offset)) :
 					u_offset;
@@ -493,9 +514,9 @@ static inline void _ipu_ch_offset_update(int ch,
 		uv_stride = stride;
 		u_offset = stride * (height - vertical_offset - 1) +
 					(stride - horizontal_offset) +
-					(uv_stride * vertical_offset) +
+					(uv_stride * vertical_offset / 2) +
 					horizontal_offset;
-		u_fix = u ? (u + (uv_stride * vertical_offset) +
+		u_fix = u ? (u + (uv_stride * vertical_offset / 2) +
 					horizontal_offset -
 					(stride * vertical_offset) - (horizontal_offset)) :
 					u_offset;
@@ -514,13 +535,19 @@ static inline void _ipu_ch_offset_update(int ch,
 	if (v_fix > v_offset)
 		v_offset = v_fix;
 
-	/* UBO and VBO are 22-bit */
+	/* UBO and VBO are 22-bit and 8-byte aligned */
 	if (u_offset/8 > 0x3fffff)
-		dev_err(g_ipu_dev,
-			"The value of U offset exceeds IPU limitation\n");
+		dev_warn(g_ipu_dev,
+			"IDMAC%d's U offset exceeds IPU limitation\n", ch);
 	if (v_offset/8 > 0x3fffff)
-		dev_err(g_ipu_dev,
-			"The value of V offset exceeds IPU limitation\n");
+		dev_warn(g_ipu_dev,
+			"IDMAC%d's V offset exceeds IPU limitation\n", ch);
+	if (u_offset%8)
+		dev_warn(g_ipu_dev,
+			"IDMAC%d's U offset is not 8-byte aligned\n", ch);
+	if (v_offset%8)
+		dev_warn(g_ipu_dev,
+			"IDMAC%d's V offset is not 8-byte aligned\n", ch);
 
 	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 0, 46, 22, u_offset / 8);
 	ipu_ch_param_mod_field(ipu_ch_param_addr(ch), 0, 68, 22, v_offset / 8);
diff --git a/drivers/mxc/ipu3/ipu_prv.h b/drivers/mxc/ipu3/ipu_prv.h
index 213ded04c87d..4e62b256889f 100644
--- a/drivers/mxc/ipu3/ipu_prv.h
+++ b/drivers/mxc/ipu3/ipu_prv.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2005-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2005-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -60,7 +60,7 @@ void _ipu_init_dc_mappings(void);
 int _ipu_dp_init(ipu_channel_t channel, uint32_t in_pixel_fmt,
 		 uint32_t out_pixel_fmt);
 void _ipu_dp_uninit(ipu_channel_t channel);
-void _ipu_dc_init(int dc_chan, int di, bool interlaced);
+void _ipu_dc_init(int dc_chan, int di, bool interlaced, uint32_t pixel_fmt);
 void _ipu_dc_uninit(int dc_chan);
 void _ipu_dp_dc_enable(ipu_channel_t channel);
 void _ipu_dp_dc_disable(ipu_channel_t channel, bool swap);
diff --git a/drivers/mxc/ipu3/ipu_regs.h b/drivers/mxc/ipu3/ipu_regs.h
index 2438df60a0ce..4a78e14df560 100644
--- a/drivers/mxc/ipu3/ipu_regs.h
+++ b/drivers/mxc/ipu3/ipu_regs.h
@@ -261,6 +261,14 @@ extern u32 *ipu_vdi_reg;
 #define DC_EVT_NEW_CHAN_R_1	9
 #define DC_EVT_NEW_DATA_R_0	10
 #define DC_EVT_NEW_DATA_R_1	11
+#define DC_EVEN_UGDE0 		12
+#define DC_ODD_UGDE0 		13
+#define DC_EVEN_UGDE1 		14
+#define DC_ODD_UGDE1 		15
+#define DC_EVEN_UGDE2 		16
+#define DC_ODD_UGDE2 		17
+#define DC_EVEN_UGDE3 		18
+#define DC_ODD_UGDE3 		19
 
 #define dc_ch_offset(ch) \
 ({ \
@@ -627,6 +635,8 @@ enum {
 	VDI_C_VWM3_CLR_2 = 0x02000000,
 	VDI_C_TOP_FIELD_MAN_1 = 0x40000000,
 	VDI_C_TOP_FIELD_AUTO_1 = 0x80000000,
+
+	DMFC_FIFO_SIZE_5F = 0x00003800,
 };
 
 enum di_pins {
@@ -654,5 +664,6 @@ enum di_sync_wave {
 
 /* DC template opcodes */
 #define WROD(lf)		(0x18 | (lf << 1))
+#define WRG	        	(0x01)
 
 #endif
diff --git a/drivers/mxc/mlb/Kconfig b/drivers/mxc/mlb/Kconfig
index 294c9776fb4d..7e3b16c2ddae 100644
--- a/drivers/mxc/mlb/Kconfig
+++ b/drivers/mxc/mlb/Kconfig
@@ -6,7 +6,7 @@ menu "MXC Media Local Bus Driver"
 
 config MXC_MLB
 	tristate "MLB support"
-	depends on ARCH_MX35
+	depends on ARCH_MX35 || ARCH_MX53
 	---help---
          Say Y to get the MLB support.
 
diff --git a/drivers/mxc/pmic/core/mc13892.c b/drivers/mxc/pmic/core/mc13892.c
index 9f232a4f5718..1175ab633fc6 100644
--- a/drivers/mxc/pmic/core/mc13892.c
+++ b/drivers/mxc/pmic/core/mc13892.c
@@ -262,6 +262,7 @@ int pmic_event_unmask(type_event event)
 
 	return ret;
 }
+EXPORT_SYMBOL(pmic_event_unmask);
 
 int pmic_event_mask(type_event event)
 {
@@ -294,7 +295,7 @@ int pmic_event_mask(type_event event)
 
 	return ret;
 }
-
+EXPORT_SYMBOL(pmic_event_mask);
 /*!
  * This function returns the PMIC version in system.
  *
diff --git a/drivers/mxc/pmic/core/pmic.h b/drivers/mxc/pmic/core/pmic.h
index 964c44a06bc1..da61b19a3f31 100644
--- a/drivers/mxc/pmic/core/pmic.h
+++ b/drivers/mxc/pmic/core/pmic.h
@@ -58,8 +58,6 @@ static inline int spi_rw(struct spi_device *spi, u8 * buf, size_t len)
 		.cs_change = 0,
 		.delay_usecs = 0,
 	};
-	mxc_spi_poll_transfer(spi, &t);
-	return 0;
 #if 0
 	struct spi_message m;
 
@@ -68,6 +66,9 @@ static inline int spi_rw(struct spi_device *spi, u8 * buf, size_t len)
 	if (spi_sync(spi, &m) != 0 || m.status != 0)
 		return PMIC_ERROR;
 	return (len - m.actual_length);
+#else
+	mxc_spi_poll_transfer(spi, &t);
+	return 0;
 #endif
 }
 
diff --git a/drivers/mxc/pmic/mc13892/pmic_adc.c b/drivers/mxc/pmic/mc13892/pmic_adc.c
index 68588a40d7e4..60ce35e86a06 100644
--- a/drivers/mxc/pmic/mc13892/pmic_adc.c
+++ b/drivers/mxc/pmic/mc13892/pmic_adc.c
@@ -17,6 +17,7 @@
 #include <linux/delay.h>
 #include <linux/wait.h>
 #include <linux/device.h>
+#include <linux/cdev.h>
 
 #include <linux/pmic_adc.h>
 #include <linux/pmic_status.h>
@@ -33,6 +34,9 @@
 #define MC13892_ADC0_TS_M_LSH	14
 #define MC13892_ADC0_TS_M_WID	3
 
+static int pmic_adc_major;
+static struct class *pmic_adc_class;
+
 /*
  * Maximun allowed variation in the three X/Y co-ordinates acquired from
  * touch-screen
@@ -924,17 +928,322 @@ static ssize_t adc_ctl(struct device *dev, struct device_attribute *attr,
 
 #endif
 
+/*!
+ * This function triggers a conversion and returns sampling results of each
+ * specified channel.
+ *
+ * @param        channels  This input parameter is bitmap to specify channels
+ *                         to be sampled.
+ * @param        result    The pointer to array to store sampling results.
+ *                         The memory should be allocated by the caller of this
+ *                         function.
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_convert_multichnnel(t_channel channels,
+					 unsigned short *result)
+{
+	t_adc_param adc_param;
+	int i;
+	PMIC_STATUS ret;
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	mc13892_adc_init_param(&adc_param);
+	pr_debug("pmic_adc_convert_multichnnel\n");
+
+	channels = channel_num[channels];
+
+	if (channels == -1) {
+		pr_debug("Wrong channel ID\n");
+		return PMIC_PARAMETER_ERROR;
+	}
+
+	adc_param.read_ts = false;
+	adc_param.single_channel = false;
+	if ((channels >= 0) && (channels <= 7)) {
+		adc_param.channel_0 = channels;
+		adc_param.channel_1 = ((channels + 4) % 4) + 4;
+	} else {
+		return PMIC_PARAMETER_ERROR;
+	}
+	adc_param.read_mode = 0x00003f;
+	adc_param.read_ts = false;
+	ret = mc13892_adc_convert(&adc_param);
+
+	for (i = 0; i <= 7; i++) {
+		result[i] = adc_param.value[i];
+	}
+	return ret;
+}
+
+/*!
+ * This function starts a Battery Current mode conversion.
+ *
+ * @param        mode      Conversion mode.
+ * @param        result    Battery Current measurement result.
+ *                         if \a mode = ADC_8CHAN_1X, the result is \n
+ *                             result[0] = (BATTP - BATT_I) \n
+ *                         if \a mode = ADC_1CHAN_8X, the result is \n
+ *                             result[0] = BATTP \n
+ *                             result[1] = BATT_I \n
+ *                             result[2] = BATTP \n
+ *                             result[3] = BATT_I \n
+ *                             result[4] = BATTP \n
+ *                             result[5] = BATT_I \n
+ *                             result[6] = BATTP \n
+ *                             result[7] = BATT_I
+ *
+ * @return       This function returns PMIC_SUCCESS if successful.
+ */
+PMIC_STATUS pmic_adc_get_battery_current(t_conversion_mode mode,
+					 unsigned short *result)
+{
+	PMIC_STATUS ret;
+	t_channel channel;
+	if (suspend_flag == 1) {
+		return -EBUSY;
+	}
+	channel = BATTERY_CURRENT;
+	if (mode == ADC_8CHAN_1X) {
+		ret = pmic_adc_convert(channel, result);
+	} else {
+		ret = pmic_adc_convert_8x(channel, result);
+	}
+	return ret;
+}
+
+/*!
+ * This function implements the open method on a MC13892 ADC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_adc_open(struct inode *inode, struct file *file)
+{
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+	pr_debug("mc13892_adc : mc13892_adc_open()\n");
+	return 0;
+}
+
+/*!
+ * This function implements the release method on a MC13892 ADC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @return       This function returns 0.
+ */
+static int pmic_adc_free(struct inode *inode, struct file *file)
+{
+	pr_debug("mc13892_adc : mc13892_adc_free()\n");
+	return 0;
+}
+
+/*!
+ * This function implements IOCTL controls on a MC13892 ADC device.
+ *
+ * @param        inode       pointer on the node
+ * @param        file        pointer on the file
+ * @param        cmd         the command
+ * @param        arg         the parameter
+ * @return       This function returns 0 if successful.
+ */
+static int pmic_adc_ioctl(struct inode *inode, struct file *file,
+			  unsigned int cmd, unsigned long arg)
+{
+	t_adc_convert_param *convert_param;
+	t_touch_mode touch_mode;
+	t_touch_screen touch_sample;
+	unsigned short b_current;
+
+	if ((_IOC_TYPE(cmd) != 'p') && (_IOC_TYPE(cmd) != 'D'))
+		return -ENOTTY;
+
+	while (suspend_flag == 1) {
+		swait++;
+		/* Block if the device is suspended */
+		if (wait_event_interruptible(suspendq, (suspend_flag == 0))) {
+			return -ERESTARTSYS;
+		}
+	}
+
+	switch (cmd) {
+	case PMIC_ADC_INIT:
+		CHECK_ERROR(pmic_adc_init());
+		break;
+
+	case PMIC_ADC_DEINIT:
+		CHECK_ERROR(pmic_adc_deinit());
+		break;
+
+	case PMIC_ADC_CONVERT:
+		if ((convert_param = kmalloc(sizeof(t_adc_convert_param),
+					     GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(convert_param, (t_adc_convert_param *) arg,
+				   sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(pmic_adc_convert(convert_param->channel,
+						   convert_param->result),
+				  (kfree(convert_param)));
+
+		if (copy_to_user((t_adc_convert_param *) arg, convert_param,
+				 sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		kfree(convert_param);
+		break;
+
+	case PMIC_ADC_CONVERT_8X:
+		if ((convert_param = kmalloc(sizeof(t_adc_convert_param),
+					     GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(convert_param, (t_adc_convert_param *) arg,
+				   sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		CHECK_ERROR_KFREE(pmic_adc_convert_8x(convert_param->channel,
+						      convert_param->result),
+				  (kfree(convert_param)));
+
+		if (copy_to_user((t_adc_convert_param *) arg, convert_param,
+				 sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		kfree(convert_param);
+		break;
+
+	case PMIC_ADC_CONVERT_MULTICHANNEL:
+		if ((convert_param = kmalloc(sizeof(t_adc_convert_param),
+					     GFP_KERNEL)) == NULL) {
+			return -ENOMEM;
+		}
+		if (copy_from_user(convert_param, (t_adc_convert_param *) arg,
+				   sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+
+		CHECK_ERROR_KFREE(pmic_adc_convert_multichnnel
+				  (convert_param->channel,
+				   convert_param->result),
+				  (kfree(convert_param)));
+
+		if (copy_to_user((t_adc_convert_param *) arg, convert_param,
+				 sizeof(t_adc_convert_param))) {
+			kfree(convert_param);
+			return -EFAULT;
+		}
+		kfree(convert_param);
+		break;
+
+	case PMIC_ADC_SET_TOUCH_MODE:
+		CHECK_ERROR(pmic_adc_set_touch_mode((t_touch_mode) arg));
+		break;
+
+	case PMIC_ADC_GET_TOUCH_MODE:
+		CHECK_ERROR(pmic_adc_get_touch_mode(&touch_mode));
+		if (copy_to_user((t_touch_mode *) arg, &touch_mode,
+				 sizeof(t_touch_mode))) {
+			return -EFAULT;
+		}
+		break;
+
+	case PMIC_ADC_GET_TOUCH_SAMPLE:
+		CHECK_ERROR(pmic_adc_get_touch_sample(&touch_sample, 1));
+		if (copy_to_user((t_touch_screen *) arg, &touch_sample,
+				 sizeof(t_touch_screen))) {
+			return -EFAULT;
+		}
+		break;
+
+	case PMIC_ADC_GET_BATTERY_CURRENT:
+		CHECK_ERROR(pmic_adc_get_battery_current(ADC_8CHAN_1X,
+							 &b_current));
+		if (copy_to_user((unsigned short *)arg, &b_current,
+				 sizeof(unsigned short))) {
+
+			return -EFAULT;
+		}
+		break;
+
+	default:
+		pr_debug("pmic_adc_ioctl: unsupported ioctl command 0x%x\n",
+			 cmd);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static struct file_operations mc13892_adc_fops = {
+	.owner = THIS_MODULE,
+	.ioctl = pmic_adc_ioctl,
+	.open = pmic_adc_open,
+	.release = pmic_adc_free,
+};
+
+static struct cdev pmic_adc_cdev;
 static DEVICE_ATTR(adc, 0644, adc_info, adc_ctl);
 
 static int pmic_adc_module_probe(struct platform_device *pdev)
 {
 	int ret = 0;
+	struct device * sdev;
+	dev_t devid;
 
 	pr_debug("PMIC ADC start probe\n");
+
+	if( (ret = alloc_chrdev_region(&devid, 0, 8, "pmic_adc")) < 0 ) {
+		pr_debug(KERN_ERR "Unable to allocate device range for pmic_adc\n");
+		return ret;
+	}
+	pmic_adc_major = MAJOR(devid);
+	if (pmic_adc_major < 0) {
+		pr_debug(KERN_ERR "Unable to get a major for pmic_adc\n");
+		ret = pmic_adc_major;
+		goto unreg_char;
+	}
+
+	cdev_init(&pmic_adc_cdev, &mc13892_adc_fops);
+	ret  =cdev_add(&pmic_adc_cdev, devid, 8);
+	if (ret < 0) {
+		pr_err("pmic_adc: cannot add character device\n");
+		goto unreg_char;
+	}
+
+	pmic_adc_class = class_create(THIS_MODULE, "pmic_adc");
+	if (IS_ERR(pmic_adc_class)) {
+		pr_debug(KERN_ERR "Error creating pmic_adc class.\n");
+		ret = PTR_ERR(pmic_adc_class);
+		goto unreg_char;
+	}
+
+	sdev = device_create(pmic_adc_class, NULL, devid, NULL, "pmic_adc");
+	if (IS_ERR(sdev) ) {
+		pr_debug(KERN_ERR "Error creating pmic_adc class device.\n");
+		ret = PTR_ERR(sdev);
+		goto cl_destroy;
+	}
+
 	ret = device_create_file(&(pdev->dev), &dev_attr_adc);
 	if (ret) {
 		pr_debug("Can't create device file!\n");
-		return -ENODEV;
+		ret = -ENODEV;
+		goto dev_destroy;
 	}
 
 	init_waitqueue_head(&suspendq);
@@ -946,11 +1255,17 @@ static int pmic_adc_module_probe(struct platform_device *pdev)
 	}
 
 	pmic_adc_ready = 1;
-	pr_debug("PMIC ADC successfully probed\n");
+	printk(KERN_DEBUG"PMIC ADC successfully probed\n");
 	return 0;
 
-      rm_dev_file:
+rm_dev_file:
 	device_remove_file(&(pdev->dev), &dev_attr_adc);
+dev_destroy:
+	device_destroy(pmic_adc_class, MKDEV(pmic_adc_major, 0));
+cl_destroy:
+	class_destroy(pmic_adc_class);
+unreg_char:
+	unregister_chrdev(pmic_adc_major, "pmic_adc");
 	return ret;
 }
 
diff --git a/drivers/mxc/pmic/mc13892/pmic_battery.c b/drivers/mxc/pmic/mc13892/pmic_battery.c
index 8535eb0a34e4..c355e0c4338f 100644
--- a/drivers/mxc/pmic/mc13892/pmic_battery.c
+++ b/drivers/mxc/pmic/mc13892/pmic_battery.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -104,6 +104,19 @@ enum chg_setting {
        VI_PROGRAM_EN
 };
 
+
+static unsigned int max_voltage_design = 3800000;
+module_param(max_voltage_design, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(max_voltage_design, "Maximum battery voltage by design.");
+
+static unsigned int min_voltage_design = 3300000;
+module_param(min_voltage_design, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(min_voltage_design, "Minimum battery voltage by design.");
+
+static unsigned int main_charger_current  = 0x8; /* 720 mA */
+module_param(main_charger_current, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(main_charger_current, "Main charge path regulator current limit.");
+
 static int pmic_set_chg_current(unsigned short curr)
 {
 	unsigned int mask;
@@ -180,15 +193,35 @@ static int pmic_set_chg_misc(enum chg_setting type, unsigned short flag)
 	return 0;
 }
 
+static void pmic_stop_charging(void)
+{
+	pmic_set_chg_misc(AUTO_CHG_DIS, 0);
+	pmic_set_chg_current(0);
+}
+
+static int pmic_restart_charging(void)
+{
+	pmic_set_chg_misc(BAT_TH_CHECK_DIS, 1);
+	pmic_set_chg_misc(AUTO_CHG_DIS, 0);
+	pmic_set_chg_misc(VI_PROGRAM_EN, 1);
+	pmic_set_chg_current(main_charger_current);
+	pmic_set_chg_misc(RESTART_CHG_STAT, 1);
+	return 0;
+}
+
 static int pmic_get_batt_voltage(unsigned short *voltage)
 {
 	t_channel channel;
 	unsigned short result[8];
 
+	pmic_stop_charging();
+
 	channel = BATTERY_VOLTAGE;
 	CHECK_ERROR(pmic_adc_convert(channel, result));
 	*voltage = result[0];
 
+	pmic_restart_charging();
+
 	return 0;
 }
 
@@ -197,10 +230,14 @@ static int pmic_get_batt_current(unsigned short *curr)
 	t_channel channel;
 	unsigned short result[8];
 
+	pmic_stop_charging();
+
 	channel = BATTERY_CURRENT;
 	CHECK_ERROR(pmic_adc_convert(channel, result));
 	*curr = result[0];
 
+	pmic_restart_charging();
+
 	return 0;
 }
 
@@ -284,16 +321,6 @@ static int pmic_get_charger_coulomb(int *coulomb)
 	return 0;
 }
 
-static int pmic_restart_charging(void)
-{
-	pmic_set_chg_misc(BAT_TH_CHECK_DIS, 1);
-	pmic_set_chg_misc(AUTO_CHG_DIS, 0);
-	pmic_set_chg_misc(VI_PROGRAM_EN, 1);
-	pmic_set_chg_current(0x8);
-	pmic_set_chg_misc(RESTART_CHG_STAT, 1);
-	return 0;
-}
-
 struct mc13892_dev_info {
 	struct device *dev;
 
@@ -353,8 +380,8 @@ static int mc13892_charger_update_status(struct mc13892_dev_info *di)
 				pmic_restart_charging();
 			} else
 				pmic_stop_coulomb_counter();
+			}
 		}
-	}
 
 	return ret;
 }
@@ -422,7 +449,7 @@ static void mc13892_battery_update_status(struct mc13892_dev_info *di)
 			else
 				di->battery_status =
 					POWER_SUPPLY_STATUS_NOT_CHARGING;
-		}
+			}
 
 		if (di->battery_status == POWER_SUPPLY_STATUS_NOT_CHARGING)
 			di->full_counter++;
@@ -491,10 +518,10 @@ static int mc13892_battery_get_property(struct power_supply *psy,
 		val->intval = di->accum_current_uAh;
 		break;
 	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
-		val->intval = 3800000;
+		val->intval = max_voltage_design;
 		break;
 	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
-		val->intval = 3300000;
+		val->intval = min_voltage_design;
 		break;
 	default:
 		return -EINVAL;
@@ -536,7 +563,7 @@ static int pmic_battery_probe(struct platform_device *pdev)
 		pr_debug("Battery driver is only applied for MC13892 V2.0\n");
 		return -1;
 	}
-	if (machine_is_mx51_babbage()) {
+	if (machine_is_mx51_babbage() || machine_is_mx50_arm2()) {
 		pr_debug("mc13892 charger is not used for this platform\n");
 		return -1;
 	}
diff --git a/drivers/mxc/security/Kconfig b/drivers/mxc/security/Kconfig
index 875848b2c69c..3e36a29ace64 100644
--- a/drivers/mxc/security/Kconfig
+++ b/drivers/mxc/security/Kconfig
@@ -27,6 +27,7 @@ config MXC_SECURITY_RNG
 	depends on ARCH_MXC
 	depends on !ARCH_MXC91321
 	depends on !ARCH_MX27
+	depends on !ARCH_MX51
 	default n
 	select MXC_SECURITY_CORE
 	---help---
diff --git a/drivers/mxc/security/sahara2/fsl_shw_auth.c b/drivers/mxc/security/sahara2/fsl_shw_auth.c
index d3100f01380a..b3f8788b553a 100644
--- a/drivers/mxc/security/sahara2/fsl_shw_auth.c
+++ b/drivers/mxc/security/sahara2/fsl_shw_auth.c
@@ -326,7 +326,7 @@ static inline fsl_shw_return_t add_assoc_preamble(sah_Head_Desc ** desc_chain,
 	return status;
 }				/* add_assoc_preamble() */
 
-#if SUPPORT_SSL
+#ifdef SUPPORT_SSL
 /*!
  * Generate an SSL value
  *
@@ -473,7 +473,7 @@ fsl_shw_return_t fsl_shw_gen_encrypt(fsl_shw_uco_t * user_ctx,
 	SAH_SF_USER_CHECK();
 
 	if (auth_ctx->mode == FSL_ACC_MODE_SSL) {
-#if SUPPORT_SSL
+#ifdef SUPPORT_SSL
 		ret = do_ssl_gen(user_ctx, auth_ctx, cipher_key_info,
 				 auth_key_info, auth_data_length, auth_data,
 				 payload_length, payload, ct, auth_value);
diff --git a/drivers/mxc/security/scc2_driver.c b/drivers/mxc/security/scc2_driver.c
index 3249405c86a1..5c0d8b4dc26d 100644
--- a/drivers/mxc/security/scc2_driver.c
+++ b/drivers/mxc/security/scc2_driver.c
@@ -415,6 +415,7 @@ extern scc_partition_status_t scc_partition_status(void *part_base)
 		break;
 	}
 }
+EXPORT_SYMBOL(scc_partition_status);
 
 /**
  * Calculate the physical address from the kernel virtual address.
@@ -427,6 +428,7 @@ uint32_t scc_virt_to_phys(void *address)
 	return (uint32_t) address - (uint32_t) scm_ram_base
 	    + (uint32_t) scm_ram_phys_base;
 }
+EXPORT_SYMBOL(scc_virt_to_phys);
 
 /**
  * Engage partition of secure memory
diff --git a/drivers/mxc/vpu/mxc_vpu.c b/drivers/mxc/vpu/mxc_vpu.c
index b9ae23928c5d..9fc25edc33fa 100644
--- a/drivers/mxc/vpu/mxc_vpu.c
+++ b/drivers/mxc/vpu/mxc_vpu.c
@@ -73,6 +73,7 @@ static struct vpu_mem_desc user_data_mem = { 0 };
 static struct vpu_mem_desc share_mem = { 0 };
 
 static void __iomem *vpu_base;
+static int vpu_irq;
 static u32 phy_vpu_base_addr;
 static struct mxc_vpu_platform_data *vpu_plat;
 
@@ -539,7 +540,7 @@ static int vpu_map_mem(struct file *fp, struct vm_area_struct *vm)
 		 request_size);
 
 	vm->vm_flags |= VM_IO | VM_RESERVED;
-	vm->vm_page_prot = pgprot_noncached(vm->vm_page_prot);
+	vm->vm_page_prot = pgprot_writecombine(vm->vm_page_prot);
 
 	return remap_pfn_range(vm, vm->vm_start, vm->vm_pgoff,
 			       request_size, vm->vm_page_prot) ? -EAGAIN : 0;
@@ -635,8 +636,9 @@ static int vpu_dev_probe(struct platform_device *pdev)
 		err = -ENXIO;
 		goto err_out_class;
 	}
+	vpu_irq = res->start;
 
-	err = request_irq(res->start, vpu_irq_handler, 0, "VPU_CODEC_IRQ",
+	err = request_irq(vpu_irq, vpu_irq_handler, 0, "VPU_CODEC_IRQ",
 			  (void *)(&vpu_data));
 	if (err)
 		goto err_out_class;
@@ -660,6 +662,7 @@ static int vpu_dev_probe(struct platform_device *pdev)
 
 static int vpu_dev_remove(struct platform_device *pdev)
 {
+	free_irq(vpu_irq, &vpu_data);
 	iounmap(vpu_base);
 	iram_free(iram.start, VPU_IRAM_SIZE);
 
@@ -690,22 +693,25 @@ static int vpu_suspend(struct platform_device *pdev, pm_message_t state)
 	for (i = 0; i < vpu_clk_usercount; i++)
 		clk_disable(vpu_clk);
 
-	clk_enable(vpu_clk);
-	if (bitwork_mem.cpu_addr != 0) {
-		SAVE_WORK_REGS;
-		SAVE_CTRL_REGS;
-		SAVE_RDWR_PTR_REGS;
-		SAVE_DIS_FLAG_REGS;
-
-		WRITE_REG(0x1, BIT_BUSY_FLAG);
-		WRITE_REG(VPU_SLEEP_REG_VALUE, BIT_RUN_COMMAND);
-		while (READ_REG(BIT_BUSY_FLAG)) ;
+	if (!cpu_is_mx37())
+	    return 0;
+	else {
+		clk_enable(vpu_clk);
+		if (bitwork_mem.cpu_addr != 0) {
+			SAVE_WORK_REGS;
+			SAVE_CTRL_REGS;
+			SAVE_RDWR_PTR_REGS;
+			SAVE_DIS_FLAG_REGS;
+
+			WRITE_REG(0x1, BIT_BUSY_FLAG);
+			WRITE_REG(VPU_SLEEP_REG_VALUE, BIT_RUN_COMMAND);
+			while (READ_REG(BIT_BUSY_FLAG))
+				;
+		}
+		clk_disable(vpu_clk);
 	}
 
-	clk_disable(vpu_clk);
-
-	if (cpu_is_mx37() || cpu_is_mx51())
-		mxc_pg_enable(pdev);
+	mxc_pg_enable(pdev);
 
 	return 0;
 
@@ -719,11 +725,12 @@ static int vpu_resume(struct platform_device *pdev)
 {
 	int i;
 
-	if (cpu_is_mx37() || cpu_is_mx51())
+	if (cpu_is_mx37())
 		mxc_pg_disable(pdev);
+	else
+	    goto recover_clk;
 
 	clk_enable(vpu_clk);
-
 	if (bitwork_mem.cpu_addr != 0) {
 		u32 *p = (u32 *) bitwork_mem.cpu_addr;
 		u32 data;
@@ -786,12 +793,13 @@ static int vpu_resume(struct platform_device *pdev)
 		WRITE_REG(VPU_WAKE_REG_VALUE, BIT_RUN_COMMAND);
 		while (READ_REG(BIT_BUSY_FLAG)) ;
 	}
-
 	clk_disable(vpu_clk);
 
+recover_clk:
 	/* Recover vpu clock */
 	for (i = 0; i < vpu_clk_usercount; i++)
 		clk_enable(vpu_clk);
+	printk("vpu_resume end\n");
 
 	return 0;
 }
@@ -824,7 +832,6 @@ static int __init vpu_init(void)
 
 static void __exit vpu_exit(void)
 {
-	free_irq(MXC_INT_VPU, (void *)(&vpu_data));
 	if (vpu_major > 0) {
 		device_destroy(vpu_class, MKDEV(vpu_major, 0));
 		class_destroy(vpu_class);
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index bd5d0e026b02..28318f4236b1 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -1885,6 +1885,13 @@ config FEC_1588
 	bool "Enable FEC 1588 timestamping"
 	depends on FEC
 
+config FEC_L2SWITCH
+	bool "L2 Switch Ethernet Controller (of ColdFire CPUs)"
+	depends on ARCH_MX28 && !FEC
+	help
+	  Say Y here if you want to use the built-in 10/100 Ethernet Switch
+	  Controller on some Motorola ColdFire processors.
+
 config FEC2
 	bool "Second FEC ethernet controller (on some ColdFire CPUs)"
 	depends on FEC
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index f3c89fb1b799..11bb1b5623bf 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -115,6 +115,7 @@ obj-$(CONFIG_HP100) += hp100.o
 obj-$(CONFIG_SMC9194) += smc9194.o
 obj-$(CONFIG_FEC) += fec.o
 obj-$(CONFIG_FEC_1588) += fec_1588.o
+obj-$(CONFIG_FEC_L2SWITCH) += fec_switch.o
 obj-$(CONFIG_FEC_MPC52xx) += fec_mpc52xx.o
 ifeq ($(CONFIG_FEC_MPC52xx_MDIO),y)
 	obj-$(CONFIG_FEC_MPC52xx) += fec_mpc52xx_phy.o
diff --git a/drivers/net/can/Kconfig b/drivers/net/can/Kconfig
index a6bf9a7fddd9..481990fd3f2b 100644
--- a/drivers/net/can/Kconfig
+++ b/drivers/net/can/Kconfig
@@ -86,8 +86,8 @@ config CAN_DEBUG_DEVICES
 
 config CAN_FLEXCAN
         tristate "Freescale FlexCAN"
-        depends on CAN && (ARCH_MX25 || ARCH_MX35 || ARCH_MX28)
-        default m
+        depends on CAN && (ARCH_MX25 || ARCH_MX35 || ARCH_MX28 || ARCH_MX53)
+        default y
         ---help---
           This select the support of Freescale CAN(FlexCAN).
           This driver can also be built as a module.
diff --git a/drivers/net/can/flexcan/dev.c b/drivers/net/can/flexcan/dev.c
index 389f85d75709..404877c33eab 100644
--- a/drivers/net/can/flexcan/dev.c
+++ b/drivers/net/can/flexcan/dev.c
@@ -35,6 +35,74 @@
 #endif
 #include "flexcan.h"
 
+#define DEFAULT_BITRATE	    500000
+#define TIME_SEGMENT_MIN    8
+#define TIME_SEGMENT_MAX    25
+#define TIME_SEGMENT_MID    ((TIME_SEGMENT_MIN + TIME_SEGMENT_MAX)/2)
+
+struct time_segment {
+	char propseg;
+	char pseg1;
+	char pseg2;
+};
+
+struct time_segment time_segments[] = {
+    { /* total 8 timequanta */
+	1, 2, 1
+    },
+    { /* total 9 timequanta */
+	1, 2, 2
+    },
+    { /* total 10 timequanta */
+	2, 2, 2
+    },
+    { /* total 11 timequanta */
+	2, 2, 3
+    },
+    { /* total 12 timequanta */
+	2, 3, 3
+    },
+    { /* total 13 timequanta */
+	3, 3, 3
+    },
+    { /* total 14 timequanta */
+	3, 3, 4
+    },
+    { /* total 15 timequanta */
+	3, 4, 4
+    },
+    { /* total 16 timequanta */
+	4, 4, 4
+    },
+    { /* total 17 timequanta */
+	4, 4, 5
+    },
+    { /* total 18 timequanta */
+	4, 5, 5
+    },
+    { /* total 19 timequanta */
+	5, 5, 5
+    },
+    { /* total 20 timequanta */
+	5, 5, 6
+    },
+    { /* total 21 timequanta */
+	5, 6, 6
+    },
+    { /* total 22 timequanta */
+	6, 6, 6
+    },
+    { /* total 23 timequanta */
+	6, 6, 7
+    },
+    { /* total 24 timequanta */
+	6, 7, 7
+    },
+    { /* total 25 timequanta */
+	7, 7, 7
+    },
+};
+
 enum {
 	FLEXCAN_ATTR_STATE = 0,
 	FLEXCAN_ATTR_BITRATE,
@@ -138,6 +206,45 @@ static void flexcan_set_bitrate(struct flexcan_device *flexcan, int bitrate)
 	 * based on the bitrate to get the timing of
 	 * presdiv, pseg1, pseg2, propseg
 	 */
+	int i, rate, div;
+	bool found = false;
+	struct time_segment *segment;
+	rate = clk_get_rate(flexcan->clk);
+
+	if (!bitrate)
+		bitrate = DEFAULT_BITRATE;
+
+	if (rate % bitrate == 0) {
+		div = rate / bitrate;
+		for (i = TIME_SEGMENT_MID; i <= TIME_SEGMENT_MAX; i++) {
+			if (div % i == 0) {
+				found = true;
+				break;
+			}
+		}
+		if (!found) {
+			for (i = TIME_SEGMENT_MID - 1;
+					    i >= TIME_SEGMENT_MIN; i--) {
+				if (div % i == 0) {
+					found = true;
+					break;
+				}
+			}
+
+		}
+	}
+
+	if (found) {
+		segment = &time_segments[i - TIME_SEGMENT_MIN];
+		flexcan->br_presdiv = div/i - 1;
+		flexcan->br_propseg = segment->propseg;
+		flexcan->br_pseg1 = segment->pseg1;
+		flexcan->br_pseg2 = segment->pseg2;
+		flexcan->bitrate = bitrate;
+	} else {
+		pr_info("The bitrate %d can't supported with clock \
+				    rate of %d \n", bitrate, rate);
+	}
 }
 
 static void flexcan_update_bitrate(struct flexcan_device *flexcan)
@@ -201,7 +308,7 @@ static int flexcan_dump_xmit_mb(struct flexcan_device *flexcan, char *buf)
 		ret +=
 		    sprintf(buf + ret,
 			    "mb[%d]::CS:0x%x ID:0x%x DATA[1~2]:0x%02x,0x%02x\n",
-			    i, flexcan->hwmb[i].mb_cs.data,
+			    i, flexcan->hwmb[i].mb_cs,
 			    flexcan->hwmb[i].mb_id, flexcan->hwmb[i].mb_data[1],
 			    flexcan->hwmb[i].mb_data[2]);
 	return ret;
@@ -214,7 +321,7 @@ static int flexcan_dump_rx_mb(struct flexcan_device *flexcan, char *buf)
 		ret +=
 		    sprintf(buf + ret,
 			    "mb[%d]::CS:0x%x ID:0x%x DATA[1~2]:0x%02x,0x%02x\n",
-			    i, flexcan->hwmb[i].mb_cs.data,
+			    i, flexcan->hwmb[i].mb_cs,
 			    flexcan->hwmb[i].mb_id, flexcan->hwmb[i].mb_data[1],
 			    flexcan->hwmb[i].mb_data[2]);
 	return ret;
@@ -575,6 +682,7 @@ struct net_device *flexcan_device_alloc(struct platform_device *pdev,
 		return NULL;
 	}
 	flexcan_device_default(flexcan);
+	flexcan_set_bitrate(flexcan, flexcan->bitrate);
 	flexcan_update_bitrate(flexcan);
 
 	num = ARRAY_SIZE(flexcan_dev_attr);
diff --git a/drivers/net/can/flexcan/flexcan.h b/drivers/net/can/flexcan/flexcan.h
index d19cc1ee0620..51a800bd8e55 100644
--- a/drivers/net/can/flexcan/flexcan.h
+++ b/drivers/net/can/flexcan/flexcan.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -32,17 +32,6 @@
 
 #define FLEXCAN_DEVICE_NAME	"FlexCAN"
 
-struct can_mb_cs {
-	unsigned int time_stamp:16;
-	unsigned int length:4;
-	unsigned int rtr:1;
-	unsigned int ide:1;
-	unsigned int srr:1;
-	unsigned int nouse1:1;
-	unsigned int code:4;
-	unsigned int nouse2:4;
-};
-
 #define CAN_MB_RX_INACTIVE	0x0
 #define CAN_MB_RX_EMPTY		0x4
 #define CAN_MB_RX_FULL		0x2
@@ -55,14 +44,24 @@ struct can_mb_cs {
 #define CAN_MB_TX_REMOTE	0xA
 
 struct can_hw_mb {
-	union {
-		struct can_mb_cs cs;
-		unsigned int data;
-	} mb_cs;
+	unsigned int mb_cs;
 	unsigned int mb_id;
 	unsigned char mb_data[8];
 };
 
+#define MB_CS_CODE_OFFSET	24
+#define MB_CS_CODE_MASK		(0xF << MB_CS_CODE_OFFSET)
+#define MB_CS_SRR_OFFSET	22
+#define MB_CS_SRR_MASK		(0x1 << MB_CS_SRR_OFFSET)
+#define MB_CS_IDE_OFFSET	21
+#define MB_CS_IDE_MASK		(0x1 << MB_CS_IDE_OFFSET)
+#define MB_CS_RTR_OFFSET	20
+#define MB_CS_RTR_MASK		(0x1 << MB_CS_RTR_OFFSET)
+#define MB_CS_LENGTH_OFFSET	16
+#define MB_CS_LENGTH_MASK	(0xF << MB_CS_LENGTH_OFFSET)
+#define MB_CS_TIMESTAMP_OFFSET	0
+#define MB_CS_TIMESTAMP_MASK	(0xFF << MB_CS_TIMESTAMP_OFFSET)
+
 #define CAN_HW_REG_MCR		0x00
 #define CAN_HW_REG_CTRL		0x04
 #define CAN_HW_REG_TIMER	0x08
diff --git a/drivers/net/can/flexcan/mbm.c b/drivers/net/can/flexcan/mbm.c
index b0341ba9128e..c846d97daadb 100644
--- a/drivers/net/can/flexcan/mbm.c
+++ b/drivers/net/can/flexcan/mbm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2008-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -55,10 +55,13 @@ static void flexcan_mb_bottom(struct net_device *dev, int index)
 
 	hwmb = flexcan->hwmb + index;
 	if (flexcan->fifo || (index >= (flexcan->maxmb - flexcan->xmit_maxmb))) {
-		if (hwmb->mb_cs.cs.code == CAN_MB_TX_ABORT)
-			hwmb->mb_cs.cs.code = CAN_MB_TX_INACTIVE;
+		if ((hwmb->mb_cs & MB_CS_CODE_MASK) >> MB_CS_CODE_OFFSET ==
+							CAN_MB_TX_ABORT) {
+			hwmb->mb_cs &= ~MB_CS_CODE_MASK;
+			hwmb->mb_cs |= CAN_MB_TX_INACTIVE << MB_CS_CODE_OFFSET;
+		}
 
-		if (hwmb->mb_cs.cs.code & CAN_MB_TX_INACTIVE) {
+		if (hwmb->mb_cs & (CAN_MB_TX_INACTIVE << MB_CS_CODE_OFFSET)) {
 			if (netif_queue_stopped(dev))
 				netif_start_queue(dev);
 			return;
@@ -68,16 +71,17 @@ static void flexcan_mb_bottom(struct net_device *dev, int index)
 	if (skb) {
 		frame = (struct can_frame *)skb_put(skb, sizeof(*frame));
 		memset(frame, 0, sizeof(*frame));
-		if (hwmb->mb_cs.cs.ide)
+		if (hwmb->mb_cs & MB_CS_IDE_MASK)
 			frame->can_id =
 			    (hwmb->mb_id & CAN_EFF_MASK) | CAN_EFF_FLAG;
 		else
 			frame->can_id = (hwmb->mb_id >> 18) & CAN_SFF_MASK;
 
-		if (hwmb->mb_cs.cs.rtr)
+		if (hwmb->mb_cs & MB_CS_RTR_MASK)
 			frame->can_id |= CAN_RTR_FLAG;
 
-		frame->can_dlc = hwmb->mb_cs.cs.length;
+		frame->can_dlc =
+		(hwmb->mb_cs & MB_CS_LENGTH_MASK) >> MB_CS_LENGTH_OFFSET;
 
 		if (frame->can_dlc && frame->can_dlc)
 			flexcan_memcpy(frame->data, hwmb->mb_data,
@@ -85,7 +89,8 @@ static void flexcan_mb_bottom(struct net_device *dev, int index)
 
 		if (flexcan->fifo
 		    || (index >= (flexcan->maxmb - flexcan->xmit_maxmb))) {
-			hwmb->mb_cs.cs.code = CAN_MB_TX_INACTIVE;
+			hwmb->mb_cs &= ~MB_CS_CODE_MASK;
+			hwmb->mb_cs |= CAN_MB_TX_INACTIVE << MB_CS_CODE_OFFSET;
 			if (netif_queue_stopped(dev))
 				netif_start_queue(dev);
 		}
@@ -101,13 +106,13 @@ static void flexcan_mb_bottom(struct net_device *dev, int index)
 		skb->ip_summed = CHECKSUM_UNNECESSARY;
 		netif_rx(skb);
 	} else {
-		tmp = hwmb->mb_cs.data;
+		tmp = hwmb->mb_cs;
 		tmp = hwmb->mb_id;
 		tmp = hwmb->mb_data[0];
 		if (flexcan->fifo
 		    || (index >= (flexcan->maxmb - flexcan->xmit_maxmb))) {
-
-			hwmb->mb_cs.cs.code = CAN_MB_TX_INACTIVE;
+			hwmb->mb_cs &= ~MB_CS_CODE_MASK;
+			hwmb->mb_cs |= CAN_MB_TX_INACTIVE << MB_CS_CODE_OFFSET;
 			if (netif_queue_stopped(dev))
 				netif_start_queue(dev);
 		}
@@ -131,17 +136,19 @@ static void flexcan_fifo_isr(struct net_device *dev, unsigned int iflag1)
 			frame =
 			    (struct can_frame *)skb_put(skb, sizeof(*frame));
 			memset(frame, 0, sizeof(*frame));
-			if (hwmb->mb_cs.cs.ide)
+			if (hwmb->mb_cs & MB_CS_IDE_MASK)
 				frame->can_id =
 				    (hwmb->mb_id & CAN_EFF_MASK) | CAN_EFF_FLAG;
 			else
 				frame->can_id =
 				    (hwmb->mb_id >> 18) & CAN_SFF_MASK;
 
-			if (hwmb->mb_cs.cs.rtr)
+			if (hwmb->mb_cs & MB_CS_RTR_MASK)
 				frame->can_id |= CAN_RTR_FLAG;
 
-			frame->can_dlc = hwmb->mb_cs.cs.length;
+			frame->can_dlc =
+				(hwmb->mb_cs & MB_CS_LENGTH_MASK) >>
+						MB_CS_LENGTH_OFFSET;
 
 			if (frame->can_dlc && (frame->can_dlc <= 8))
 				flexcan_memcpy(frame->data, hwmb->mb_data,
@@ -158,7 +165,7 @@ static void flexcan_fifo_isr(struct net_device *dev, unsigned int iflag1)
 			skb->ip_summed = CHECKSUM_UNNECESSARY;
 			netif_rx(skb);
 		} else {
-			tmp = hwmb->mb_cs.data;
+			tmp = hwmb->mb_cs;
 			tmp = hwmb->mb_id;
 			tmp = hwmb->mb_data[0];
 			tmp = __raw_readl(flexcan->io_base + CAN_HW_REG_TIMER);
@@ -252,7 +259,8 @@ int flexcan_mbm_xmit(struct flexcan_device *flexcan, struct can_frame *frame)
 	struct can_hw_mb *hwmb = flexcan->hwmb;
 
 	do {
-		if (hwmb[i].mb_cs.cs.code == CAN_MB_TX_INACTIVE)
+		if ((hwmb[i].mb_cs & MB_CS_CODE_MASK) >> MB_CS_CODE_OFFSET ==
+							    CAN_MB_TX_INACTIVE)
 			break;
 		if ((++i) > flexcan->maxmb) {
 			if (flexcan->fifo)
@@ -273,22 +281,24 @@ int flexcan_mbm_xmit(struct flexcan_device *flexcan, struct can_frame *frame)
 	}
 
 	if (frame->can_id & CAN_RTR_FLAG)
-		hwmb[i].mb_cs.cs.rtr = 1;
+		hwmb[i].mb_cs |= 1 << MB_CS_RTR_OFFSET;
 	else
-		hwmb[i].mb_cs.cs.rtr = 0;
+		hwmb[i].mb_cs &= ~MB_CS_RTR_MASK;
 
 	if (frame->can_id & CAN_EFF_FLAG) {
-		hwmb[i].mb_cs.cs.ide = 1;
-		hwmb[i].mb_cs.cs.srr = 1;
+		hwmb[i].mb_cs |= 1 << MB_CS_IDE_OFFSET;
+		hwmb[i].mb_cs |= 1 << MB_CS_SRR_OFFSET;
 		hwmb[i].mb_id = frame->can_id & CAN_EFF_MASK;
 	} else {
-		hwmb[i].mb_cs.cs.ide = 0;
+		hwmb[i].mb_cs &= ~MB_CS_IDE_MASK;
 		hwmb[i].mb_id = (frame->can_id & CAN_SFF_MASK) << 18;
 	}
 
-	hwmb[i].mb_cs.cs.length = frame->can_dlc;
+	hwmb[i].mb_cs &= ~MB_CS_LENGTH_MASK;
+	hwmb[i].mb_cs |= frame->can_dlc << MB_CS_LENGTH_OFFSET;
 	flexcan_memcpy(hwmb[i].mb_data, frame->data, frame->can_dlc);
-	hwmb[i].mb_cs.cs.code = CAN_MB_TX_ONCE;
+	hwmb[i].mb_cs &= ~MB_CS_CODE_MASK;
+	hwmb[i].mb_cs |= CAN_MB_TX_ONCE << MB_CS_CODE_OFFSET;
 	return 0;
 }
 
@@ -325,23 +335,27 @@ void flexcan_mbm_init(struct flexcan_device *flexcan)
 			id_table[i] = 0;
 	} else {
 		for (i = 0; i < rx_mb; i++) {
-			hwmb[i].mb_cs.cs.code = CAN_MB_RX_EMPTY;
+			hwmb[i].mb_cs &= ~MB_CS_CODE_MASK;
+			hwmb[i].mb_cs |= CAN_MB_RX_EMPTY << MB_CS_CODE_OFFSET;
 			/*
 			 * IDE bit can not control by mask registers
 			 * So set message buffer to receive extend
 			 * or standard message.
 			 */
-			if (flexcan->ext_msg && flexcan->std_msg)
-				hwmb[i].mb_cs.cs.ide = i & 1;
-			else {
+			if (flexcan->ext_msg && flexcan->std_msg) {
+				hwmb[i].mb_cs &= ~MB_CS_IDE_MASK;
+				hwmb[i].mb_cs |= (i & 1) << MB_CS_IDE_OFFSET;
+			} else {
 				if (flexcan->ext_msg)
-					hwmb[i].mb_cs.cs.ide = 1;
+					hwmb[i].mb_cs |= 1 << MB_CS_IDE_OFFSET;
 			}
 		}
 	}
 
-	for (; i <= flexcan->maxmb; i++)
-		hwmb[i].mb_cs.cs.code = CAN_MB_TX_INACTIVE;
+	for (; i <= flexcan->maxmb; i++) {
+		hwmb[i].mb_cs &= ~MB_CS_CODE_MASK;
+		hwmb[i].mb_cs |= CAN_MB_TX_INACTIVE << MB_CS_CODE_OFFSET;
+	}
 
 	flexcan->xmit_mb = rx_mb;
 }
diff --git a/drivers/net/enc28j60.c b/drivers/net/enc28j60.c
index 582eb37390ed..8a34f4679fa5 100644
--- a/drivers/net/enc28j60.c
+++ b/drivers/net/enc28j60.c
@@ -30,8 +30,6 @@
 #include <linux/delay.h>
 #include <linux/spi/spi.h>
 
-#include <mach/platform.h>
-
 #include "enc28j60_hw.h"
 
 #define DRV_NAME	"enc28j60"
@@ -53,9 +51,17 @@
 #define MAX_TX_RETRYCOUNT	16
 
 #ifdef CONFIG_ARCH_STMP3XXX
+#include <mach/platform.h>
 #include <mach/stmp3xxx.h>
 #include <mach/regs-ocotp.h>
 #endif
+#ifdef CONFIG_ARCH_MXS
+#include <mach/system.h>
+#include <mach/hardware.h>
+#include <mach/regs-ocotp.h>
+#define REGS_OCOTP_BASE	IO_ADDRESS(OCOTP_PHYS_ADDR)
+#endif
+
 enum {
 	RXFILTER_NORMAL,
 	RXFILTER_MULTI,
@@ -104,12 +110,14 @@ static int enc28j60_get_mac(unsigned char *dev_addr, int idx)
 		return false;
 
 	if (!mac[idx]) {
-#ifdef CONFIG_ARCH_STMP3XXX
+#if defined(CONFIG_ARCH_STMP3XXX) || defined(CONFIG_ARCH_MXS)
 		if (get_evk_board_version() >= 1) {
 			int mac1 , mac2 , retry = 0;
 
-			stmp3xxx_setl(BM_OCOTP_CTRL_RD_BANK_OPEN, REGS_OCOTP_BASE + HW_OCOTP_CTRL);
-			while (__raw_readl(REGS_OCOTP_BASE + HW_OCOTP_CTRL) & BM_OCOTP_CTRL_BUSY) {
+			__raw_writel(BM_OCOTP_CTRL_RD_BANK_OPEN,
+					REGS_OCOTP_BASE + HW_OCOTP_CTRL_SET);
+			while (__raw_readl(REGS_OCOTP_BASE + HW_OCOTP_CTRL) &
+					BM_OCOTP_CTRL_BUSY) {
 				msleep(10);
 				retry++;
 				if (retry > 10)
diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index 46799e092bc1..206be36f0d19 100644
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -296,6 +296,17 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		bufaddr = fep->tx_bounce[index];
 	}
 
+	if (fep->ptimer_present) {
+		if (fec_ptp_do_txstamp(skb))
+			estatus = BD_ENET_TX_TS;
+		else
+			estatus = 0;
+#ifdef CONFIG_FEC_1588
+		bdp->cbd_esc = (estatus | BD_ENET_TX_INT);
+		bdp->cbd_bdu = 0;
+#endif
+	}
+
 #ifdef CONFIG_ARCH_MXS
 	swap_buffer(bufaddr, skb->len);
 #endif
@@ -318,16 +329,6 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			| BD_ENET_TX_LAST | BD_ENET_TX_TC);
 	bdp->cbd_sc = status;
 
-	if (fep->ptimer_present) {
-		if (fec_ptp_do_txstamp(skb))
-			estatus = BD_ENET_TX_TS;
-		else
-			estatus = 0;
-#ifdef CONFIG_FEC_1588
-		bdp->cbd_esc = (estatus | BD_ENET_TX_INT);
-		bdp->cbd_bdu = 0;
-#endif
-	}
 	dev->trans_start = jiffies;
 
 	/* Trigger transmission start */
@@ -807,7 +808,7 @@ static struct mii_bus *fec_enet_mii_init(struct platform_device *pdev)
 {
 	struct net_device *dev = platform_get_drvdata(pdev);
 	struct fec_enet_private *fep = netdev_priv(dev);
-	struct fec_enet_platform_data *pdata;
+	struct fec_platform_data *pdata;
 	int err = -ENXIO, i;
 
 	fep->mii_timeout = 0;
@@ -836,6 +837,7 @@ static struct mii_bus *fec_enet_mii_init(struct platform_device *pdev)
 	fep->mii_bus->priv = fep;
 	fep->mii_bus->parent = &pdev->dev;
 	pdata = pdev->dev.platform_data;
+	fep->mii_bus->phy_mask = pdata->phy_mask;
 
 	fep->mii_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
 	if (!fep->mii_bus->irq) {
@@ -1131,17 +1133,18 @@ fec_set_mac_address(struct net_device *dev, void *p)
 {
 	struct fec_enet_private *fep = netdev_priv(dev);
 	struct sockaddr *addr = p;
+	u32 temp_mac[2];
 
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 
 	memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
 
-	writel(dev->dev_addr[3] | (dev->dev_addr[2] << 8) |
-		(dev->dev_addr[1] << 16) | (dev->dev_addr[0] << 24),
-		fep->hwp + FEC_ADDR_LOW);
-	writel((dev->dev_addr[5] << 16) | (dev->dev_addr[4] << 24),
-		fep + FEC_ADDR_HIGH);
+	memcpy(&temp_mac, dev->dev_addr, ETH_ALEN);
+
+	writel(cpu_to_be32(temp_mac[0]), fep->hwp + FEC_ADDR_LOW);
+	writel(cpu_to_be32(temp_mac[1]), fep->hwp + FEC_ADDR_HIGH);
+
 	return 0;
 }
 
@@ -1277,6 +1280,7 @@ fec_restart(struct net_device *dev, int duplex)
 	/* Clear any outstanding interrupt. */
 	writel(0xffc00000, fep->hwp + FEC_IEVENT);
 
+#if !defined(CONFIG_MACH_CCMX51JS) && !defined(CONFIG_MACH_CCWMX51JS)
 	/* Reset all multicast.	*/
 	writel(0, fep->hwp + FEC_GRP_HASH_TABLE_HIGH);
 	writel(0, fep->hwp + FEC_GRP_HASH_TABLE_LOW);
@@ -1284,6 +1288,7 @@ fec_restart(struct net_device *dev, int duplex)
 	writel(0, fep->hwp + FEC_HASH_TABLE_HIGH);
 	writel(0, fep->hwp + FEC_HASH_TABLE_LOW);
 #endif
+#endif /* !defined(CONFIG_MACH_CCMX51JS) && !defined(CONFIG_MACH_CCWMX51JS) */
 
 #ifndef CONFIG_ARCH_MXS
 	if (fep->phy_interface == PHY_INTERFACE_MODE_RMII) {
@@ -1407,6 +1412,15 @@ fec_stop(struct net_device *dev)
 	writel(1, fep->hwp + FEC_ECNTRL);
 	udelay(10);
 
+#ifdef CONFIG_ARCH_MXS
+	/* Check MII or RMII */
+	if (fep->phy_interface == PHY_INTERFACE_MODE_RMII)
+		writel(readl(fep->hwp + FEC_R_CNTRL) | 0x100,
+					fep->hwp + FEC_R_CNTRL);
+	else
+		writel(readl(fep->hwp + FEC_R_CNTRL) & ~0x100,
+					fep->hwp + FEC_R_CNTRL);
+#endif
 	/* Clear outstanding MII command interrupts. */
 	writel(FEC_ENET_MII, fep->hwp + FEC_IEVENT);
 
@@ -1482,6 +1496,18 @@ fec_probe(struct platform_device *pdev)
 		fep->phy_interface = pdata->phy;
 		if (pdata->init && pdata->init())
 			goto failed_platform_init;
+
+		/*
+		 * The priority for getting MAC address is:
+		 * (1) kernel command line fec_mac = xx:xx:xx...
+		 * (2) platform data mac field got from fuse etc
+		 * (3) bootloader set the FEC mac register
+		 */
+
+		if (!is_valid_ether_addr(fec_mac_default) &&
+			pdata->mac && is_valid_ether_addr(pdata->mac))
+			memcpy(fec_mac_default, pdata->mac,
+						sizeof(fec_mac_default));
 	} else
 		fep->phy_interface = PHY_INTERFACE_MODE_MII;
 
@@ -1499,10 +1525,10 @@ fec_probe(struct platform_device *pdev)
 		fep->mii_bus = fec_mii_bus;
 	}
 
-	fep->ptp_priv = kmalloc(sizeof(struct fec_ptp_private), GFP_KERNEL);
+	fep->ptp_priv = kzalloc(sizeof(struct fec_ptp_private), GFP_KERNEL);
 	if (fep->ptp_priv) {
 		fep->ptp_priv->hwp = fep->hwp;
-		ret = fec_ptp_init(fep->ptp_priv);
+		ret = fec_ptp_init(fep->ptp_priv, pdev->id);
 		if (ret)
 			printk(KERN_WARNING
 					"IEEE1588: ptp-timer is unavailable\n");
diff --git a/drivers/net/fec_1588.c b/drivers/net/fec_1588.c
index 5babcc29de78..c4bf278c7f19 100644
--- a/drivers/net/fec_1588.c
+++ b/drivers/net/fec_1588.c
@@ -35,7 +35,7 @@
 static DECLARE_WAIT_QUEUE_HEAD(ptp_rx_ts_wait);
 #define PTP_GET_RX_TIMEOUT      (HZ/10)
 
-static struct fec_ptp_private *ptp_private;
+static struct fec_ptp_private *ptp_private[2];
 
 /* Alloc the ring resource */
 static int fec_ptp_init_circ(struct circ_buf *ptp_buf)
@@ -88,14 +88,15 @@ static int fec_ptp_is_full(struct circ_buf *buf)
 }
 
 static int fec_ptp_insert(struct circ_buf *ptp_buf,
-				struct fec_ptp_data_t *data)
+			  struct fec_ptp_data_t *data,
+			  struct fec_ptp_private *priv)
 {
 	struct fec_ptp_data_t *tmp;
 
 	if (fec_ptp_is_full(ptp_buf))
 		return 1;
 
-	spin_lock(&ptp_private->ptp_lock);
+	spin_lock(&priv->ptp_lock);
 	tmp = (struct fec_ptp_data_t *)(ptp_buf->buf) + ptp_buf->tail;
 
 	tmp->key = data->key;
@@ -104,13 +105,15 @@ static int fec_ptp_insert(struct circ_buf *ptp_buf,
 
 	ptp_buf->tail = fec_ptp_calc_index(DEFAULT_PTP_RX_BUF_SZ,
 					ptp_buf->tail, 1);
-	spin_unlock(&ptp_private->ptp_lock);
+	spin_unlock(&priv->ptp_lock);
 
 	return 0;
 }
 
 static int fec_ptp_find_and_remove(struct circ_buf *ptp_buf,
-			int key, struct fec_ptp_data_t *data)
+				   int key,
+				   struct fec_ptp_data_t *data,
+				   struct fec_ptp_private *priv)
 {
 	int i;
 	int size = DEFAULT_PTP_RX_BUF_SZ;
@@ -129,10 +132,10 @@ static int fec_ptp_find_and_remove(struct circ_buf *ptp_buf,
 		i = fec_ptp_calc_index(size, i, 1);
 	}
 
-	spin_lock_irqsave(&ptp_private->ptp_lock, flags);
+	spin_lock_irqsave(&priv->ptp_lock, flags);
 	if (i == end) {
 		ptp_buf->head = end;
-		spin_unlock_irqrestore(&ptp_private->ptp_lock, flags);
+		spin_unlock_irqrestore(&priv->ptp_lock, flags);
 		return 1;
 	}
 
@@ -140,7 +143,7 @@ static int fec_ptp_find_and_remove(struct circ_buf *ptp_buf,
 	data->ts_time.nsec = tmp->ts_time.nsec;
 
 	ptp_buf->head = fec_ptp_calc_index(size, i, 1);
-	spin_unlock_irqrestore(&ptp_private->ptp_lock, flags);
+	spin_unlock_irqrestore(&priv->ptp_lock, flags);
 
 	return 0;
 }
@@ -154,9 +157,9 @@ int fec_ptp_start(struct fec_ptp_private *priv)
 	writel(FEC_T_CTRL_RESTART, fpp->hwp + FEC_ATIME_CTRL);
 	writel(FEC_T_INC_40MHZ << FEC_T_INC_OFFSET, fpp->hwp + FEC_ATIME_INC);
 	writel(FEC_T_PERIOD_ONE_SEC, fpp->hwp + FEC_ATIME_EVT_PERIOD);
-	writel(FEC_T_CTRL_PERIOD_RST, fpp->hwp + FEC_ATIME_CTRL);
 	/* start counter */
-	writel(FEC_T_CTRL_ENABLE, fpp->hwp + FEC_ATIME_CTRL);
+	writel(FEC_T_CTRL_PERIOD_RST | FEC_T_CTRL_ENABLE,
+			fpp->hwp + FEC_ATIME_CTRL);
 
 	return 0;
 }
@@ -175,12 +178,19 @@ void fec_ptp_stop(struct fec_ptp_private *priv)
 static void fec_get_curr_cnt(struct fec_ptp_private *priv,
 			struct ptp_rtc_time *curr_time)
 {
+	u32 tempval;
+
+	writel(FEC_T_CTRL_CAPTURE, priv->hwp + FEC_ATIME_CTRL);
 	writel(FEC_T_CTRL_CAPTURE, priv->hwp + FEC_ATIME_CTRL);
 	curr_time->rtc_time.nsec = readl(priv->hwp + FEC_ATIME);
 	curr_time->rtc_time.sec = priv->prtc;
+
 	writel(FEC_T_CTRL_CAPTURE, priv->hwp + FEC_ATIME_CTRL);
-	if (readl(priv->hwp + FEC_ATIME) < curr_time->rtc_time.nsec)
-		curr_time->rtc_time.sec++;
+	tempval = readl(priv->hwp + FEC_ATIME);
+	if (tempval < curr_time->rtc_time.nsec) {
+		curr_time->rtc_time.nsec = tempval;
+		curr_time->rtc_time.sec = priv->prtc;
+	}
 }
 
 /* Set the 1588 timer counter registers */
@@ -190,12 +200,12 @@ static void fec_set_1588cnt(struct fec_ptp_private *priv,
 	u32 tempval;
 	unsigned long flags;
 
-	spin_lock_irqsave(&ptp_private->cnt_lock, flags);
+	spin_lock_irqsave(&priv->cnt_lock, flags);
 	priv->prtc = fec_time->rtc_time.sec;
 
 	tempval = fec_time->rtc_time.nsec;
 	writel(tempval, priv->hwp + FEC_ATIME);
-	spin_unlock_irqrestore(&ptp_private->cnt_lock, flags);
+	spin_unlock_irqrestore(&priv->cnt_lock, flags);
 }
 
 /* Set the BD to ptp */
@@ -207,11 +217,11 @@ int fec_ptp_do_txstamp(struct sk_buff *skb)
 	if (skb->len > 44) {
 		/* Check if port is 319 for PTP Event, and check for UDP */
 		iph = ip_hdr(skb);
-		if (iph->protocol != FEC_PACKET_TYPE_UDP)
+		if (iph == NULL || iph->protocol != FEC_PACKET_TYPE_UDP)
 			return 0;
 
 		udph = udp_hdr(skb);
-		if (udph->source == 319)
+		if (udph != NULL && ntohs(udph->source) == 319)
 			return 1;
 	}
 
@@ -244,24 +254,24 @@ void fec_ptp_store_rxstamp(struct fec_ptp_private *priv,
 		return;
 
 	udph = (struct udphdr *)(skb->data + FEC_PTP_UDP_OFFS);
-	if (udph->source != 319)
+	if (ntohs(udph->source) != 319)
 		return;
 
 	seq_id = *((u16 *)(skb->data + FEC_PTP_SEQ_ID_OFFS));
 	control = *((u8 *)(skb->data + FEC_PTP_CTRL_OFFS));
 
-	tmp_rx_time.key = seq_id;
+	tmp_rx_time.key = ntohs(seq_id);
 	tmp_rx_time.ts_time.sec = fpp->prtc;
 	tmp_rx_time.ts_time.nsec = bdp->ts;
 
 	switch (control) {
 
 	case PTP_MSG_SYNC:
-		fec_ptp_insert(&(priv->rx_time_sync), &tmp_rx_time);
+		fec_ptp_insert(&(priv->rx_time_sync), &tmp_rx_time, priv);
 		break;
 
 	case PTP_MSG_DEL_REQ:
-		fec_ptp_insert(&(priv->rx_time_del_req), &tmp_rx_time);
+		fec_ptp_insert(&(priv->rx_time_del_req), &tmp_rx_time, priv);
 		break;
 
 	/* clear transportSpecific field*/
@@ -271,11 +281,11 @@ void fec_ptp_store_rxstamp(struct fec_ptp_private *priv,
 		switch (msg_type) {
 		case PTP_MSG_P_DEL_REQ:
 			fec_ptp_insert(&(priv->rx_time_pdel_req),
-						&tmp_rx_time);
+						&tmp_rx_time, priv);
 			break;
 		case PTP_MSG_P_DEL_RESP:
 			fec_ptp_insert(&(priv->rx_time_pdel_resp),
-					&tmp_rx_time);
+					&tmp_rx_time, priv);
 			break;
 		default:
 			break;
@@ -308,20 +318,20 @@ static uint8_t fec_get_rx_time(struct fec_ptp_private *priv,
 	switch (mode) {
 	case PTP_MSG_SYNC:
 		flag = fec_ptp_find_and_remove(&(priv->rx_time_sync),
-						key, &tmp);
+						key, &tmp, priv);
 		break;
 	case PTP_MSG_DEL_REQ:
 		flag = fec_ptp_find_and_remove(&(priv->rx_time_del_req),
-						key, &tmp);
+						key, &tmp, priv);
 		break;
 
 	case PTP_MSG_P_DEL_REQ:
 		flag = fec_ptp_find_and_remove(&(priv->rx_time_pdel_req),
-						key, &tmp);
+						key, &tmp, priv);
 		break;
 	case PTP_MSG_P_DEL_RESP:
 		flag = fec_ptp_find_and_remove(&(priv->rx_time_pdel_resp),
-						key, &tmp);
+						key, &tmp, priv);
 		break;
 
 	default:
@@ -341,19 +351,19 @@ static uint8_t fec_get_rx_time(struct fec_ptp_private *priv,
 		switch (mode) {
 		case PTP_MSG_SYNC:
 			flag = fec_ptp_find_and_remove(&(priv->rx_time_sync),
-				key, &tmp);
+				key, &tmp, priv);
 			break;
 		case PTP_MSG_DEL_REQ:
 			flag = fec_ptp_find_and_remove(
-				&(priv->rx_time_del_req), key, &tmp);
+				&(priv->rx_time_del_req), key, &tmp, priv);
 			break;
 		case PTP_MSG_P_DEL_REQ:
 			flag = fec_ptp_find_and_remove(
-				&(priv->rx_time_pdel_req), key, &tmp);
+				&(priv->rx_time_pdel_req), key, &tmp, priv);
 			break;
 		case PTP_MSG_P_DEL_RESP:
 			flag = fec_ptp_find_and_remove(
-				&(priv->rx_time_pdel_resp), key, &tmp);
+				&(priv->rx_time_pdel_resp), key, &tmp, priv);
 			break;
 		}
 
@@ -367,6 +377,75 @@ static uint8_t fec_get_rx_time(struct fec_ptp_private *priv,
 	}
 }
 
+static void fec_handle_ptpdrift(struct ptp_set_comp *comp,
+				struct ptp_time_correct *ptc)
+{
+	u32 ndrift;
+	u32 i;
+	u32 tmp, tmp_ns, tmp_prid;
+	u32 min_ns, min_prid, miss_ns;
+
+	ndrift = comp->drift;
+	if (ndrift == 0) {
+		ptc->corr_inc = 0;
+		ptc->corr_period = 0;
+		return;
+	}
+
+	if (ndrift >= FEC_ATIME_40MHZ) {
+		ptc->corr_inc = (u32)(ndrift / FEC_ATIME_40MHZ);
+		ptc->corr_period = 1;
+		return;
+	}
+
+	min_ns = 1;
+	tmp = FEC_ATIME_40MHZ % ndrift;
+	tmp_prid = (u32)(FEC_ATIME_40MHZ / ndrift);
+	min_prid = tmp_prid;
+	miss_ns = tmp / tmp_prid;
+	for (i = 2; i <= FEC_T_INC_40MHZ; i++) {
+		tmp = (FEC_ATIME_40MHZ * i) % ndrift;
+		tmp_prid = (FEC_ATIME_40MHZ * i) / ndrift;
+		tmp_ns = tmp / tmp_prid;
+		if (tmp_ns <= 10) {
+			min_ns = i;
+			min_prid = tmp_prid;
+			break;
+		}
+		if (tmp_ns < miss_ns) {
+			min_ns = i;
+			min_prid = tmp_prid;
+			miss_ns = tmp_ns;
+		}
+	}
+
+	ptc->corr_inc = min_ns;
+	ptc->corr_period = min_prid;
+}
+
+static void fec_set_drift(struct fec_ptp_private *priv,
+			  struct ptp_set_comp *comp)
+{
+	struct ptp_time_correct	tc;
+	struct fec_ptp_private *fpp = priv;
+	u32 tmp, corr_ns;
+
+	fec_handle_ptpdrift(comp, &tc);
+	if (tc.corr_inc == 0)
+		return;
+
+	if (comp->o_ops == TRUE)
+		corr_ns = FEC_T_INC_40MHZ + tc.corr_inc;
+	else
+		corr_ns = FEC_T_INC_40MHZ - tc.corr_inc;
+
+	tmp = readl(fpp->hwp + FEC_ATIME_INC) & FEC_T_INC_MASK;
+	tmp |= corr_ns << FEC_T_INC_CORR_OFFSET;
+	writel(tmp, fpp->hwp + FEC_ATIME_INC);
+
+	writel(tc.corr_period, fpp->hwp + FEC_ATIME_CORR);
+}
+
 static int ptp_open(struct inode *inode, struct file *file)
 {
 	return 0;
@@ -387,10 +466,12 @@ static int ptp_ioctl(
 	struct ptp_rtc_time curr_time;
 	struct ptp_time rx_time, tx_time;
 	struct ptp_ts_data *p_ts;
+	struct ptp_set_comp *p_comp;
 	struct fec_ptp_private *priv;
+	unsigned int minor = MINOR(inode->i_rdev);
 	int retval = 0;
 
-	priv = (struct fec_ptp_private *) ptp_private;
+	priv = (struct fec_ptp_private *) ptp_private[minor];
 	switch (cmd) {
 	case PTP_GET_RX_TIMESTAMP:
 		p_ts = (struct ptp_ts_data *)arg;
@@ -428,10 +509,11 @@ static int ptp_ioctl(
 		priv->rx_time_pdel_resp.tail = 0;
 		break;
 	case PTP_SET_COMPENSATION:
-		/* TBD */
+		p_comp = (struct ptp_set_comp *)arg;
+		fec_set_drift(priv, p_comp);
 		break;
 	case PTP_GET_ORIG_COMP:
-		/* TBD */
+		((struct ptp_get_comp *)arg)->dw_origcomp = FEC_PTP_ORIG_COMP;
 		break;
 	default:
 		return -EINVAL;
@@ -466,7 +548,7 @@ static void ptp_free(void)
 /*
  * Resource required for accessing 1588 Timer Registers.
  */
-int fec_ptp_init(struct fec_ptp_private *priv)
+int fec_ptp_init(struct fec_ptp_private *priv, int id)
 {
 	fec_ptp_init_circ(&(priv->rx_time_sync));
 	fec_ptp_init_circ(&(priv->rx_time_del_req));
@@ -475,8 +557,9 @@ int fec_ptp_init(struct fec_ptp_private *priv)
 
 	spin_lock_init(&priv->ptp_lock);
 	spin_lock_init(&priv->cnt_lock);
-	ptp_private = priv;
-	init_ptp();
+	ptp_private[id] = priv;
+	if (id == 0)
+		init_ptp();
 	return 0;
 }
 EXPORT_SYMBOL(fec_ptp_init);
diff --git a/drivers/net/fec_1588.h b/drivers/net/fec_1588.h
index 55b1a8c995bd..800ff310668f 100644
--- a/drivers/net/fec_1588.h
+++ b/drivers/net/fec_1588.h
@@ -24,6 +24,9 @@
 
 #include <linux/circ_buf.h>
 
+#define FALSE			0
+#define TRUE			1
+
 /* FEC 1588 register bits */
 #define FEC_T_CTRL_CAPTURE		0x00000800
 #define FEC_T_CTRL_RESTART		0x00000200
@@ -32,8 +35,11 @@
 
 #define FEC_T_INC_MASK			0x0000007f
 #define FEC_T_INC_OFFSET		0
+#define FEC_T_INC_CORR_MASK		0x00007f00
+#define FEC_T_INC_CORR_OFFSET		8
 
-#define FEC_T_INC_40MHZ			20
+#define FEC_T_INC_40MHZ			25
+#define FEC_ATIME_40MHZ			40000000
 
 #define FEC_T_PERIOD_ONE_SEC		0x3B9ACA00
 
@@ -52,7 +58,7 @@
 #define PTP_MSG_ALL_OTHER		0x5
 
 #define PTP_GET_TX_TIMESTAMP		0x1
-#define PTP_GET_RX_TIMESTAMP		0x2
+#define PTP_GET_RX_TIMESTAMP		0x9
 #define PTP_SET_RTC_TIME		0x3
 #define PTP_SET_COMPENSATION		0x4
 #define PTP_GET_CURRENT_TIME		0x5
@@ -64,13 +70,15 @@
 #define PTP_GET_RX_TIMESTAMP_PDELAY_RESP	0xD
 
 #define FEC_PTP_DOMAIN_DLFT		0xe0000181
-#define FEC_PTP_IP_OFFS			0xE
-#define FEC_PTP_UDP_OFFS		0x22
-#define FEC_PTP_MSG_TYPE_OFFS		0x2A
-#define FEC_PTP_SEQ_ID_OFFS		0x48
-#define FEC_PTP_CTRL_OFFS		0x4A
+#define FEC_PTP_IP_OFFS			0x0
+#define FEC_PTP_UDP_OFFS		0x14
+#define FEC_PTP_MSG_TYPE_OFFS		0x1C
+#define FEC_PTP_SEQ_ID_OFFS		0x3A
+#define FEC_PTP_CTRL_OFFS		0x3C
 #define FEC_PACKET_TYPE_UDP		0x11
 
+#define FEC_PTP_ORIG_COMP		0x15555555
+
 /* PTP standard time representation structure */
 struct ptp_time{
 	u64 sec;	/* seconds */
@@ -102,6 +110,31 @@ struct ptp_rtc_time {
 	struct ptp_time rtc_time;
 };
 
+/* interface for PTP driver command SET_COMPENSATION */
+struct ptp_set_comp {
+	u32 drift;
+	bool o_ops;
+};
+
+/* interface for PTP driver command GET_ORIG_COMP */
+struct ptp_get_comp {
+	/* the initial compensation value */
+	u32 dw_origcomp;
+	/* the minimum compensation value */
+	u32 dw_mincomp;
+	/*the max compensation value*/
+	u32 dw_maxcomp;
+	/*the min drift applying min compensation value in ppm*/
+	u32 dw_mindrift;
+	/*the max drift applying max compensation value in ppm*/
+	u32 dw_maxdrift;
+};
+
+struct ptp_time_correct {
+	u32 corr_period;
+	u32 corr_inc;
+};
+
 /* PTP message version */
 #define PTP_1588_MSG_VER_1	1
 #define PTP_1588_MSG_VER_2	2
@@ -124,7 +157,7 @@ struct fec_ptp_private {
 };
 
 #ifdef CONFIG_FEC_1588
-extern int fec_ptp_init(struct fec_ptp_private *priv);
+extern int fec_ptp_init(struct fec_ptp_private *priv, int id);
 extern void fec_ptp_cleanup(struct fec_ptp_private *priv);
 extern int fec_ptp_start(struct fec_ptp_private *priv);
 extern void fec_ptp_stop(struct fec_ptp_private *priv);
@@ -134,7 +167,7 @@ extern void fec_ptp_store_rxstamp(struct fec_ptp_private *priv,
 				  struct sk_buff *skb,
 				  struct bufdesc *bdp);
 #else
-static inline int fec_ptp_init(struct fec_ptp_private *priv)
+static inline int fec_ptp_init(struct fec_ptp_private *priv, int id)
 {
 	return 1;
 }
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
index bd4e8d72dc08..e17b70291bbc 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -264,6 +264,8 @@ static int mdio_bus_match(struct device *dev, struct device_driver *drv)
 		(phydev->phy_id & phydrv->phy_id_mask));
 }
 
+#ifdef CONFIG_PM
+
 static bool mdio_bus_phy_may_suspend(struct phy_device *phydev)
 {
 	struct device_driver *drv = phydev->dev.driver;
@@ -295,34 +297,88 @@ static bool mdio_bus_phy_may_suspend(struct phy_device *phydev)
 	return true;
 }
 
-/* Suspend and resume.  Copied from platform_suspend and
- * platform_resume
- */
-static int mdio_bus_suspend(struct device * dev, pm_message_t state)
+static int mdio_bus_suspend(struct device *dev)
 {
 	struct phy_driver *phydrv = to_phy_driver(dev->driver);
 	struct phy_device *phydev = to_phy_device(dev);
 
+	/*
+	 * We must stop the state machine manually, otherwise it stops out of
+	 * control, possibly with the phydev->lock held. Upon resume, netdev
+	 * may call phy routines that try to grab the same lock, and that may
+	 * lead to a deadlock.
+	 */
+	if (phydev->attached_dev)
+		phy_stop_machine(phydev);
+
 	if (!mdio_bus_phy_may_suspend(phydev))
 		return 0;
+
 	return phydrv->suspend(phydev);
 }
 
-static int mdio_bus_resume(struct device * dev)
+static int mdio_bus_resume(struct device *dev)
 {
 	struct phy_driver *phydrv = to_phy_driver(dev->driver);
 	struct phy_device *phydev = to_phy_device(dev);
+	int ret;
 
 	if (!mdio_bus_phy_may_suspend(phydev))
+		goto no_resume;
+
+	ret = phydrv->resume(phydev);
+	if (ret < 0)
+		return ret;
+
+no_resume:
+	if (phydev->attached_dev)
+		phy_start_machine(phydev, NULL);
+
+	return 0;
+}
+
+static int mdio_bus_restore(struct device *dev)
+{
+	struct phy_device *phydev = to_phy_device(dev);
+	struct net_device *netdev = phydev->attached_dev;
+	int ret;
+
+	if (!netdev)
 		return 0;
-	return phydrv->resume(phydev);
+
+	ret = phy_init_hw(phydev);
+	if (ret < 0)
+		return ret;
+
+	/* The PHY needs to renegotiate. */
+	phydev->link = 0;
+	phydev->state = PHY_UP;
+
+	phy_start_machine(phydev, NULL);
+
+	return 0;
 }
 
+static struct dev_pm_ops mdio_bus_pm_ops = {
+	.suspend = mdio_bus_suspend,
+	.resume = mdio_bus_resume,
+	.freeze = mdio_bus_suspend,
+	.thaw = mdio_bus_resume,
+	.restore = mdio_bus_restore,
+};
+
+#define MDIO_BUS_PM_OPS (&mdio_bus_pm_ops)
+
+#else
+
+#define MDIO_BUS_PM_OPS NULL
+
+#endif /* CONFIG_PM */
+
 struct bus_type mdio_bus_type = {
 	.name		= "mdio_bus",
 	.match		= mdio_bus_match,
-	.suspend	= mdio_bus_suspend,
-	.resume		= mdio_bus_resume,
+	.pm		= MDIO_BUS_PM_OPS,
 };
 EXPORT_SYMBOL(mdio_bus_type);
 
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index eda94fcd4065..d2df6382e123 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -413,7 +413,6 @@ EXPORT_SYMBOL(phy_start_aneg);
 
 
 static void phy_change(struct work_struct *work);
-static void phy_state_machine(struct work_struct *work);
 
 /**
  * phy_start_machine - start PHY state machine tracking
@@ -433,7 +432,6 @@ void phy_start_machine(struct phy_device *phydev,
 {
 	phydev->adjust_state = handler;
 
-	INIT_DELAYED_WORK(&phydev->state_queue, phy_state_machine);
 	schedule_delayed_work(&phydev->state_queue, HZ);
 }
 
@@ -764,7 +762,7 @@ EXPORT_SYMBOL(phy_start);
  * phy_state_machine - Handle the state machine
  * @work: work_struct that describes the work to be done
  */
-static void phy_state_machine(struct work_struct *work)
+void phy_state_machine(struct work_struct *work)
 {
 	struct delayed_work *dwork = to_delayed_work(work);
 	struct phy_device *phydev =
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index b10fedd82143..adbc0fded130 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -177,6 +177,7 @@ struct phy_device* phy_device_create(struct mii_bus *bus, int addr, int phy_id)
 	dev->state = PHY_DOWN;
 
 	mutex_init(&dev->lock);
+	INIT_DELAYED_WORK(&dev->state_queue, phy_state_machine);
 
 	return dev;
 }
@@ -378,6 +379,20 @@ void phy_disconnect(struct phy_device *phydev)
 }
 EXPORT_SYMBOL(phy_disconnect);
 
+int phy_init_hw(struct phy_device *phydev)
+{
+	int ret;
+
+	if (!phydev->drv || !phydev->drv->config_init)
+		return 0;
+
+	ret = phy_scan_fixups(phydev);
+	if (ret < 0)
+		return ret;
+
+	return phydev->drv->config_init(phydev);
+}
+
 /**
  * phy_attach_direct - attach a network device to a given PHY device pointer
  * @dev: network device to attach
@@ -425,21 +440,7 @@ int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 	/* Do initial configuration here, now that
 	 * we have certain key parameters
 	 * (dev_flags and interface) */
-	if (phydev->drv->config_init) {
-		int err;
-
-		err = phy_scan_fixups(phydev);
-
-		if (err < 0)
-			return err;
-
-		err = phydev->drv->config_init(phydev);
-
-		if (err < 0)
-			return err;
-	}
-
-	return 0;
+	return phy_init_hw(phydev);
 }
 EXPORT_SYMBOL(phy_attach_direct);
 
diff --git a/drivers/net/smsc911x.c b/drivers/net/smsc911x.c
index d4c82f5fa555..a13d107a412b 100644
--- a/drivers/net/smsc911x.c
+++ b/drivers/net/smsc911x.c
@@ -789,6 +789,7 @@ static void smsc911x_phy_adjust_link(struct net_device *dev)
 		}
 		pdata->last_carrier = carrier;
 	}
+	udelay(10);
 }
 
 static int smsc911x_mii_probe(struct net_device *dev)
@@ -1011,7 +1012,7 @@ static int smsc911x_poll(struct napi_struct *napi, int budget)
 	struct net_device *dev = pdata->dev;
 	int npackets = 0;
 
-	while (likely(netif_running(dev)) && (npackets < budget)) {
+	while (npackets < budget) {
 		unsigned int pktlength;
 		unsigned int pktwords;
 		struct sk_buff *skb;
@@ -1584,7 +1585,7 @@ static irqreturn_t smsc911x_irqhandler(int irq, void *dev_id)
 	if (unlikely(intsts & inten & INT_STS_PHY_INT_)) {
 		smsc911x_reg_write( pdata, INT_STS , INT_STS_PHY_INT_);
 		temp = smsc911x_mii_read(phy_dev->bus, phy_dev->addr, MII_INTSTS);
-		SMSC_TRACE("PHY interrupt, sts 0x%04X", (u16)temp);
+		SMSC_TRACE(DRV,"PHY interrupt, sts 0x%04X", (u16)temp);
 		smsc911x_phy_adjust_link(dev);
 		serviced = IRQ_HANDLED;
 	}
diff --git a/drivers/net/wireless/ath6kl/os/linux/ar6000_android.c b/drivers/net/wireless/ath6kl/os/linux/ar6000_android.c
index 1ca77e513493..9d0c3773d4d7 100644
--- a/drivers/net/wireless/ath6kl/os/linux/ar6000_android.c
+++ b/drivers/net/wireless/ath6kl/os/linux/ar6000_android.c
@@ -56,7 +56,7 @@ ATH_DEBUG_INSTANTIATE_MODULE_VAR(android,
 #endif
 
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
-char fwpath[256] = "/system/wifi";
+char fwpath[256] = "/lib/firmware/ath6k/AR6102";
 #endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0) */
 int buspm = WLAN_PWR_CTRL_CUT_PWR;
 int wow2mode = WLAN_PWR_CTRL_CUT_PWR;
diff --git a/drivers/power/mxs/Makefile b/drivers/power/mxs/Makefile
index 6662defd9c70..c7675a9ec52b 100644
--- a/drivers/power/mxs/Makefile
+++ b/drivers/power/mxs/Makefile
@@ -5,5 +5,5 @@
 obj-$(CONFIG_BATTERY_MXS) += mxs-battery.o
 
 mxs-battery-objs := ddi_bc_api.o ddi_bc_hw.o ddi_bc_init.o \
-		         ddi_bc_ramp.o ddi_bc_sm.o ddi_power_battery.o linux.o
+		         ddi_bc_ramp.o ddi_bc_sm.o ddi_power_battery.o linux.o fiq.o
 
diff --git a/drivers/power/mxs/ddi_bc_internal.h b/drivers/power/mxs/ddi_bc_internal.h
index a8510d08935c..b5bceeffae98 100644
--- a/drivers/power/mxs/ddi_bc_internal.h
+++ b/drivers/power/mxs/ddi_bc_internal.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2010 Freescale Semiconductor, Inc.
  */
 
 /*
@@ -41,6 +41,7 @@
 
 /* Externs */
 
+#include <linux/kernel.h>
 
 extern bool g_ddi_bc_Configured;
 extern ddi_bc_Cfg_t g_ddi_bc_Configuration;
diff --git a/drivers/power/mxs/ddi_power_battery.c b/drivers/power/mxs/ddi_power_battery.c
index 6e2119af1676..762f29bd784e 100644
--- a/drivers/power/mxs/ddi_power_battery.c
+++ b/drivers/power/mxs/ddi_power_battery.c
@@ -1805,9 +1805,11 @@ void ddi_power_enable_vddio_interrupt(bool enable)
 
 }
 
+
 void ddi_power_handle_vddio_brnout(void)
 {
-	if (ddi_power_GetPmu5vStatus() == new_5v_connection) {
+	if (ddi_power_GetPmu5vStatus() == new_5v_connection ||
+		(ddi_power_GetPmu5vStatus() == new_5v_disconnection)) {
 		ddi_power_enable_vddio_interrupt(false);
 	} else {
 #ifdef DEBUG_IRQS
diff --git a/drivers/power/mxs/fiq.S b/drivers/power/mxs/fiq.S
index ee71730c85c9..1ad380d07efd 100644
--- a/drivers/power/mxs/fiq.S
+++ b/drivers/power/mxs/fiq.S
@@ -19,11 +19,10 @@
 
 #include <linux/linkage.h>
 #include <asm/assembler.h>
-#include <mach/platform.h>
 #include <mach/hardware.h>
 #include <asm/pgtable-hwdef.h>
 #include <mach/regs-power.h>
-#include <mach/regs-clkctrl.h>
+#include <mach/../../regs-clkctrl.h>
 #include <mach/regs-timrot.h>
 
 	.align 5
@@ -33,7 +32,6 @@
 	.globl lock_vector_tlb
 
 power_fiq_start:
-
 	ldr 	r8,power_reg
 	ldr	r9,[r8,#HW_POWER_CTRL ]
 	ldr	r10,power_off
@@ -101,7 +99,7 @@ check_dcdc4p2:
 	subs    pc,lr, #4
 
 power_reg:
-	.long REGS_POWER_BASE
+	.long IO_ADDRESS(POWER_PHYS_ADDR)
 power_off:
 	.long 0x3e770001
 power_bo:
diff --git a/drivers/power/mxs/linux.c b/drivers/power/mxs/linux.c
index 6a3172415145..1c2dfc10f7ca 100644
--- a/drivers/power/mxs/linux.c
+++ b/drivers/power/mxs/linux.c
@@ -102,7 +102,7 @@ struct mxs_info {
 #define IRQ_DCDC4P2_BRNOUT IRQ_DCDC4P2_BO
 #endif
 
-/* #define  POWER_FIQ */
+#define  POWER_FIQ
 
 /* #define DEBUG_IRQS */
 
@@ -129,9 +129,7 @@ void init_protection(struct mxs_info *info)
 	battery_voltage = ddi_power_GetBattery();
 
 	/* InitializeFiqSystem(); */
-#ifdef CONFIG_ARCH_MX23
 	ddi_power_InitOutputBrownouts();
-#endif
 
 
 	/* if we start the kernel with 4p2 already started
@@ -238,12 +236,12 @@ static void check_and_handle_5v_connection(struct mxs_info *info)
 		*/
 		if ((__raw_readl(REGS_POWER_BASE + HW_POWER_5VCTRL)
 			& BM_POWER_5VCTRL_CHARGE_4P2_ILIMIT) ==
-			(0x8 << BP_POWER_5VCTRL_CHARGE_4P2_ILIMIT)) {
+			(0x20 << BP_POWER_5VCTRL_CHARGE_4P2_ILIMIT)) {
 			dev_info(info->dev, "waiting USB enum done...\r\n");
 		}
 		while ((__raw_readl(REGS_POWER_BASE + HW_POWER_5VCTRL)
 			& BM_POWER_5VCTRL_CHARGE_4P2_ILIMIT)
-			== (0x8 << BP_POWER_5VCTRL_CHARGE_4P2_ILIMIT)) {
+			== (0x20 << BP_POWER_5VCTRL_CHARGE_4P2_ILIMIT)) {
 			msleep(50);
 		}
 	#endif
@@ -299,7 +297,7 @@ static void check_and_handle_5v_connection(struct mxs_info *info)
 				__raw_writel(__raw_readl(REGS_POWER_BASE +
 				HW_POWER_5VCTRL) &
 				(~BM_POWER_5VCTRL_CHARGE_4P2_ILIMIT)
-				| (0x8 << BP_POWER_5VCTRL_CHARGE_4P2_ILIMIT),
+				| (0x20 << BP_POWER_5VCTRL_CHARGE_4P2_ILIMIT),
 				REGS_POWER_BASE + HW_POWER_5VCTRL);
 
 			}
@@ -659,6 +657,8 @@ static irqreturn_t mxs_irq_batt_brnout(int irq, void *cookie)
 #endif
 	return IRQ_HANDLED;
 }
+
+
 static irqreturn_t mxs_irq_vddd_brnout(int irq, void *cookie)
 {
 #ifdef DEBUG_IRQS
@@ -1144,13 +1144,13 @@ static int __init mxs_bat_init(void)
 
 #ifdef CONFIG_MXS_VBUS_CURRENT_DRAW
 	if (((__raw_readl(REGS_POWER_BASE + HW_POWER_5VCTRL) &
-		BM_POWER_5VCTRL_CHARGE_4P2_ILIMIT) == 0x8000)
+		BM_POWER_5VCTRL_CHARGE_4P2_ILIMIT) == 0x20000)
 		&& ((__raw_readl(REGS_POWER_BASE + HW_POWER_5VCTRL) &
 		BM_POWER_5VCTRL_PWD_CHARGE_4P2) == 0)) {
 #ifdef CONFIG_USB_GADGET
 		printk(KERN_INFO "USB GADGET exist,wait USB enum done...\r\n");
 		while (((__raw_readl(REGS_POWER_BASE + HW_POWER_5VCTRL)
-			& BM_POWER_5VCTRL_CHARGE_4P2_ILIMIT) == 0x8000) &&
+			& BM_POWER_5VCTRL_CHARGE_4P2_ILIMIT) == 0x20000) &&
 			((__raw_readl(REGS_POWER_BASE + HW_POWER_5VCTRL) &
 			BM_POWER_5VCTRL_PWD_CHARGE_4P2) == 0))
 			;
@@ -1161,8 +1161,7 @@ static int __init mxs_bat_init(void)
 	}
 	cpu = clk_get(NULL, "cpu");
 	pll0 = clk_get(NULL, "ref_cpu");
-	if (cpu->set_parent)
-		cpu->set_parent(cpu, pll0);
+	clk_set_parent(cpu, pll0);
 #endif
 	return platform_driver_register(&mxs_batdrv);
 }
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index fd3183e30a1c..804c32cabb50 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -153,4 +153,9 @@ config REGULATOR_MC9S08DZ60
 	depends on MXC_PMIC_MC9S08DZ60
 	default y
 
+config REGULATOR_MAX17135
+	tristate "Maxim MAX17135 Regulator Support"
+	depends on REGULATOR
+	default n
+
 endif
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index a10178d99acc..d88116ba8139 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_REGULATOR_WM8350) += wm8350-regulator.o
 obj-$(CONFIG_REGULATOR_WM8400) += wm8400-regulator.o
 obj-$(CONFIG_REGULATOR_DA903X)	+= da903x.o
 obj-$(CONFIG_REGULATOR_PCF50633) += pcf50633-regulator.o
+obj-$(CONFIG_REGULATOR_MAX17135) += max17135-regulator.o
 
 obj-$(CONFIG_REGULATOR_MC13892) += reg-mc13892.o
 obj-$(CONFIG_REGULATOR_MC13783) += reg-mc13783.o
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 09492700cddf..bd320c1ab70c 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -109,6 +109,16 @@ config RTC_INTF_DEV_UIE_EMUL
 	  clock several times per second, please enable this option
 	  only if you know that you really need it.
 
+config RTC_INTF_ALARM
+	bool "Android alarm driver"
+	depends on RTC_CLASS
+	default y
+	help
+	  Provides non-wakeup and rtc backed wakeup alarms based on rtc or
+	  elapsed realtime, and a non-wakeup alarm on the monotonic clock.
+	  Also provides an ioctl to set the wall time which must be used
+	  for elapsed realtime to work.
+
 config RTC_DRV_TEST
 	tristate "Test driver/device"
 	help
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 91da97eca589..c8df86258348 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -11,6 +11,7 @@ obj-$(CONFIG_RTC_HCTOSYS)	+= hctosys.o
 obj-$(CONFIG_RTC_CLASS)		+= rtc-core.o
 rtc-core-y			:= class.o interface.o
 
+rtc-core-$(CONFIG_RTC_INTF_ALARM) += alarm.o
 rtc-core-$(CONFIG_RTC_INTF_DEV)	+= rtc-dev.o
 rtc-core-$(CONFIG_RTC_INTF_PROC) += rtc-proc.o
 rtc-core-$(CONFIG_RTC_INTF_SYSFS) += rtc-sysfs.o
diff --git a/drivers/rtc/rtc-mxc_v2.c b/drivers/rtc/rtc-mxc_v2.c
index 5d410fd9d5d2..beb31415a4da 100644
--- a/drivers/rtc/rtc-mxc_v2.c
+++ b/drivers/rtc/rtc-mxc_v2.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2004-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -37,6 +37,10 @@
 #include <linux/uaccess.h>
 #include <mach/hardware.h>
 #include <asm/io.h>
+#include <linux/mxc_srtc.h>
+
+
+#define SRTC_LPSCLR_LLPSC_LSH	17	 /* start bit for LSB time value */
 
 #define SRTC_LPPDR_INIT       0x41736166	/* init for glitch detect */
 
@@ -147,6 +151,12 @@ struct rtc_drv_data {
 	bool irq_enable;
 };
 
+
+/* completion event for implementing RTC_WAIT_FOR_TIME_SET ioctl */
+DECLARE_COMPLETION(srtc_completion);
+/* global to save difference of 47-bit counter value */
+static int64_t time_diff;
+
 /*!
  * @defgroup RTC Real Time Clock (RTC) Driver
  */
@@ -313,6 +323,8 @@ static int mxc_rtc_ioctl(struct device *dev, unsigned int cmd,
 	void __iomem *ioaddr = pdata->ioaddr;
 	unsigned long lock_flags = 0;
 	u32 lp_cr;
+	u64 time_47bit;
+	int retVal;
 
 	switch (cmd) {
 	case RTC_AIE_OFF:
@@ -339,6 +351,36 @@ static int mxc_rtc_ioctl(struct device *dev, unsigned int cmd,
 		__raw_writel(lp_cr, ioaddr + SRTC_LPCR);
 		spin_unlock_irqrestore(&rtc_lock, lock_flags);
 		return 0;
+
+	case RTC_READ_TIME_47BIT:
+		time_47bit = (((u64) __raw_readl(ioaddr + SRTC_LPSCMR)) << 32 |
+			((u64) __raw_readl(ioaddr + SRTC_LPSCLR)));
+		time_47bit >>= SRTC_LPSCLR_LLPSC_LSH;
+
+		if (arg && copy_to_user((u64 *) arg, &time_47bit, sizeof(u64)))
+			return -EFAULT;
+
+		return 0;
+
+	case RTC_WAIT_TIME_SET:
+
+		/* don't block without releasing mutex first */
+		mutex_unlock(&pdata->rtc->ops_lock);
+
+		/* sleep until awakened by SRTC driver when LPSCMR is changed */
+		wait_for_completion(&srtc_completion);
+
+		/* relock mutex because rtc_dev_ioctl will unlock again */
+		retVal = mutex_lock_interruptible(&pdata->rtc->ops_lock);
+
+		/* copy the new time difference = new time - previous time
+		  * to the user param. The difference is a signed value */
+		if (arg && copy_to_user((int64_t *) arg, &time_diff,
+			sizeof(int64_t)))
+			return -EFAULT;
+
+		return retVal;
+
 	}
 
 	return -ENOIOCTLCMD;
@@ -372,14 +414,31 @@ static int mxc_rtc_set_time(struct device *dev, struct rtc_time *tm)
 	struct rtc_drv_data *pdata = dev_get_drvdata(dev);
 	void __iomem *ioaddr = pdata->ioaddr;
 	unsigned long time;
+	u64 old_time_47bit, new_time_47bit;
 	int ret;
 	ret = rtc_tm_to_time(tm, &time);
 	if (ret != 0)
 		return ret;
 
+	old_time_47bit = (((u64) __raw_readl(ioaddr + SRTC_LPSCMR)) << 32 |
+			((u64) __raw_readl(ioaddr + SRTC_LPSCLR)));
+	old_time_47bit >>= SRTC_LPSCLR_LLPSC_LSH;
+
 	__raw_writel(time, ioaddr + SRTC_LPSCMR);
 	rtc_write_sync_lp(ioaddr);
 
+	new_time_47bit = (((u64) __raw_readl(ioaddr + SRTC_LPSCMR)) << 32 |
+			((u64) __raw_readl(ioaddr + SRTC_LPSCLR)));
+	new_time_47bit >>= SRTC_LPSCLR_LLPSC_LSH;
+
+	/* update the difference between previous time and new time */
+	time_diff = new_time_47bit - old_time_47bit;
+
+	/* signal all waiting threads that time changed */
+	complete_all(&srtc_completion);
+	/* reinitialize completion variable */
+	INIT_COMPLETION(srtc_completion);
+
 	return 0;
 }
 
@@ -549,41 +608,30 @@ static int mxc_rtc_probe(struct platform_device *pdev)
 
 	/* clear lp interrupt status */
 	__raw_writel(0xFFFFFFFF, ioaddr + SRTC_LPSR);
-	udelay(100);;
+	udelay(100);
 
 	plat_data = (struct mxc_srtc_platform_data *)pdev->dev.platform_data;
-	clk = clk_get(NULL, "iim_clk");
-	clk_enable(clk);
-	srtc_secmode_addr = ioremap(plat_data->srtc_sec_mode_addr, 1);
-
-	/* Check SRTC security mode */
-	if (((__raw_readl(srtc_secmode_addr) & SRTC_SECMODE_MASK) ==
-	    SRTC_SECMODE_LOW) && (cpu_is_mx51_rev(CHIP_REV_1_0) == 1)) {
-		/* Workaround for MX51 TO1 due to inaccurate CKIL clock */
-		__raw_writel(SRTC_LPCR_EN_LP, ioaddr + SRTC_LPCR);
-		udelay(100);
-	} else {
-		/* move out of init state */
-		__raw_writel((SRTC_LPCR_IE | SRTC_LPCR_NSA),
-			     ioaddr + SRTC_LPCR);
 
-		udelay(100);
+	/* move out of init state */
+	__raw_writel((SRTC_LPCR_IE | SRTC_LPCR_NSA),
+		     ioaddr + SRTC_LPCR);
 
-		while ((__raw_readl(ioaddr + SRTC_LPSR) & SRTC_LPSR_IES) == 0);
+	udelay(100);
 
-		/* move out of non-valid state */
-		__raw_writel((SRTC_LPCR_IE | SRTC_LPCR_NVE | SRTC_LPCR_NSA |
-			      SRTC_LPCR_EN_LP), ioaddr + SRTC_LPCR);
+	while ((__raw_readl(ioaddr + SRTC_LPSR) & SRTC_LPSR_IES) == 0)
+		;
 
-		udelay(100);
+	/* move out of non-valid state */
+	__raw_writel((SRTC_LPCR_IE | SRTC_LPCR_NVE | SRTC_LPCR_NSA |
+		      SRTC_LPCR_EN_LP), ioaddr + SRTC_LPCR);
 
-		while ((__raw_readl(ioaddr + SRTC_LPSR) & SRTC_LPSR_NVES) == 0);
+	udelay(100);
 
-		__raw_writel(0xFFFFFFFF, ioaddr + SRTC_LPSR);
-		udelay(100);
-	}
-	clk_disable(clk);
-	clk_put(clk);
+	while ((__raw_readl(ioaddr + SRTC_LPSR) & SRTC_LPSR_NVES) == 0)
+		;
+
+	__raw_writel(0xFFFFFFFF, ioaddr + SRTC_LPSR);
+	udelay(100);
 
 	rtc = rtc_device_register(pdev->name, &pdev->dev,
 				  &mxc_rtc_ops, THIS_MODULE);
diff --git a/drivers/rtc/rtc-mxs.c b/drivers/rtc/rtc-mxs.c
index 0e2b0e1e14f6..bb4c33b1a0ba 100644
--- a/drivers/rtc/rtc-mxs.c
+++ b/drivers/rtc/rtc-mxs.c
@@ -254,6 +254,8 @@ static int mxs_rtc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, rtc_data);
 
+	device_init_wakeup(&pdev->dev, 1);
+
 	return 0;
 }
 
diff --git a/drivers/serial/Kconfig b/drivers/serial/Kconfig
index 869337a7fd68..b41db4d22436 100644
--- a/drivers/serial/Kconfig
+++ b/drivers/serial/Kconfig
@@ -373,6 +373,24 @@ config SERIAL_MXS_AUART
 	help
 	  Driver for Freescale i.MXS internal application serial port
 
+config SERIAL_MXS_AUART_CONSOLE
+       bool "Support for console on i.MXS application serial port"
+       depends on SERIAL_MXS_AUART=y
+       select SERIAL_CORE_CONSOLE
+       ---help---
+         Say Y here if you wish to use the i.MXS app serial port as the
+         system console (the system console is the device which receives all
+         kernel messages and warnings and which allows logins in single user
+         mode).
+
+         Even if you say Y here, the currently visible framebuffer console
+         (/dev/tty0) will still be used as the system console by default, but
+         you can alter that using a kernel command line option such as
+         "console=ttySP1". (Try "man bootparam" or see the documentation of
+         your boot loader (lilo or loadlin) about how to pass options to the
+         kernel at boot time.)
+
+
 config SERIAL_MXS_DUART_CONSOLE
         bool "Support for console on i.MXS debug serial port"
         depends on SERIAL_MXS_DUART=y
diff --git a/drivers/serial/mxs-auart.c b/drivers/serial/mxs-auart.c
index 0eea46d71979..63d7d9128efc 100644
--- a/drivers/serial/mxs-auart.c
+++ b/drivers/serial/mxs-auart.c
@@ -19,6 +19,7 @@
 #include <linux/device.h>
 #include <linux/errno.h>
 #include <linux/init.h>
+#include <linux/console.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
 #include <linux/slab.h>
@@ -47,6 +48,8 @@
 #define MXS_AUART_MAJOR	242
 #define MXS_AUART_RX_THRESHOLD 16
 
+static struct uart_driver auart_driver;
+
 struct mxs_auart_port {
 	struct uart_port port;
 
@@ -514,7 +517,7 @@ static void mxs_auart_settermios(struct uart_port *u,
 
 	/* parity */
 	if (cflag & PARENB) {
-		ctrl |= BM_UARTAPP_LINECTRL_PEN | BM_UARTAPP_LINECTRL_SPS;
+		ctrl |= BM_UARTAPP_LINECTRL_PEN;
 		if ((cflag & PARODD) == 0)
 			ctrl |= BM_UARTAPP_LINECTRL_EPS;
 	}
@@ -565,9 +568,33 @@ static irqreturn_t mxs_auart_irq_handle(int irq, void *context)
 		mxs_auart_tx_chars(s);
 		istat &= ~BM_UARTAPP_INTR_TXIS;
 	}
-	if (istat & 0xFFFF)
+	/* modem status interrupt bits are undefined
+	after reset,and the hardware do not support
+	DSRMIS,DCDMIS and RIMIS bit,so we should ingore
+	them when they are pending. */
+	if (istat & (BM_UARTAPP_INTR_ABDIS
+		| BM_UARTAPP_INTR_OEIS
+		| BM_UARTAPP_INTR_BEIS
+		| BM_UARTAPP_INTR_PEIS
+		| BM_UARTAPP_INTR_FEIS
+		| BM_UARTAPP_INTR_RTIS
+		| BM_UARTAPP_INTR_TXIS
+		| BM_UARTAPP_INTR_RXIS
+		| BM_UARTAPP_INTR_CTSMIS)) {
 		dev_info(s->dev, "Unhandled status %x\n", istat);
-	__raw_writel(istatus & 0xFFFF,
+	}
+	__raw_writel(istatus & (BM_UARTAPP_INTR_ABDIS
+		| BM_UARTAPP_INTR_OEIS
+		| BM_UARTAPP_INTR_BEIS
+		| BM_UARTAPP_INTR_PEIS
+		| BM_UARTAPP_INTR_FEIS
+		| BM_UARTAPP_INTR_RTIS
+		| BM_UARTAPP_INTR_TXIS
+		| BM_UARTAPP_INTR_RXIS
+		| BM_UARTAPP_INTR_DSRMIS
+		| BM_UARTAPP_INTR_DCDMIS
+		| BM_UARTAPP_INTR_CTSMIS
+		| BM_UARTAPP_INTR_RIMIS),
 			s->port.membase + HW_UARTAPP_INTR_CLR);
 
 	return IRQ_HANDLED;
@@ -761,7 +788,160 @@ static struct uart_ops mxs_auart_ops = {
 	.config_port    = mxs_auart_config_port,
 	.verify_port    = mxs_auart_verify_port,
 };
+#ifdef CONFIG_SERIAL_MXS_AUART_CONSOLE
+static struct mxs_auart_port auart_port[CONFIG_MXS_AUART_PORTS] = {};
+
+static void
+auart_console_write(struct console *co, const char *s, unsigned int count)
+{
+	struct uart_port *port;
+	unsigned int status, old_cr;
+	int i;
+
+	if (co->index >	CONFIG_MXS_AUART_PORTS || co->index < 0)
+		return;
+
+	port = &auart_port[co->index].port;
+
+	/* First save the CR then disable the interrupts */
+	old_cr = __raw_readl(port->membase + HW_UARTAPP_CTRL2);
+	__raw_writel(BM_UARTAPP_CTRL2_UARTEN | BM_UARTAPP_CTRL2_TXE,
+		     port->membase + HW_UARTAPP_CTRL2_SET);
+
+	/* Now, do each character */
+	for (i = 0; i < count; i++) {
+		do {
+			status = __raw_readl(port->membase + HW_UARTAPP_STAT);
+		} while (status & BM_UARTAPP_STAT_TXFF);
+
+		__raw_writel(s[i], port->membase + HW_UARTAPP_DATA);
+		if (s[i] == '\n') {
+			do {
+				status = __raw_readl(port->membase +
+						     HW_UARTAPP_STAT);
+			} while (status & BM_UARTAPP_STAT_TXFF);
+			__raw_writel('\r', port->membase + HW_UARTAPP_DATA);
+		}
+	}
+
+	/*
+	 *      Finally, wait for transmitter to become empty
+	 *      and restore the TCR
+	 */
+	do {
+		status = __raw_readl(port->membase + HW_UARTAPP_STAT);
+	} while (status & BM_UARTAPP_STAT_BUSY);
+	__raw_writel(old_cr, port->membase + HW_UARTAPP_CTRL2);
+}
+
+static void __init
+auart_console_get_options(struct uart_port *port, int *baud,
+			  int *parity, int *bits)
+{
+	if (__raw_readl(port->membase + HW_UARTAPP_CTRL2)
+				& BM_UARTAPP_CTRL2_UARTEN) {
+		unsigned int lcr_h, quot;
+		lcr_h = __raw_readl(port->membase + HW_UARTAPP_LINECTRL);
+
+		*parity = 'n';
+		if (lcr_h & BM_UARTAPP_LINECTRL_PEN) {
+			if (lcr_h & BM_UARTAPP_LINECTRL_EPS)
+				*parity = 'e';
+			else
+				*parity = 'o';
+		}
+
+		if ((lcr_h & BM_UARTAPP_LINECTRL_WLEN)
+				== BF_UARTAPP_LINECTRL_WLEN(2))
+			*bits = 7;
+		else
+			*bits = 8;
+
+		quot = (((__raw_readl(port->membase + HW_UARTAPP_LINECTRL)
+				& BM_UARTAPP_LINECTRL_BAUD_DIVINT))
+				    >> (BP_UARTAPP_LINECTRL_BAUD_DIVINT - 6))
+			| (((__raw_readl(port->membase + HW_UARTAPP_LINECTRL)
+				& BM_UARTAPP_LINECTRL_BAUD_DIVFRAC))
+					>> BP_UARTAPP_LINECTRL_BAUD_DIVFRAC);
+		if (quot == 0)
+			quot = 1;
+		*baud = (port->uartclk << 2) / quot;
+	}
+}
+
+static int __init auart_console_setup(struct console *co, char *options)
+{
+	struct mxs_auart_port *port;
+	int baud = 115200;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+	/*
+	 * Check whether an invalid uart number has been specified, and
+	 * if so, search for the first available port that does have
+	 * console support.
+	 */
+	if (co->index > CONFIG_MXS_AUART_PORTS || co->index < 0)
+		return -EINVAL;
+
+	port = &auart_port[co->index].port;
+
+	if (port->port.membase == 0) {
+		if (cpu_is_mx23()) {
+			if (co->index == 1) {
+				port->port.membase = IO_ADDRESS(0x8006C000);
+				port->port.mapbase = 0x8006C000;
+			} else {
+				port->port.membase = IO_ADDRESS(0x8006E000);
+				port->port.mapbase = 0x8006E000;
+			}
+		}
+
+		port->port.fifosize = 16;
+		port->port.ops = &mxs_auart_ops;
+		port->port.flags = ASYNC_BOOT_AUTOCONF;
+		port->port.line = 0;
+	}
+	mxs_auart_reset(port);
+
+	__raw_writel(BM_UARTAPP_CTRL2_UARTEN,
+		port->port.membase + HW_UARTAPP_CTRL2_SET);
+
+	if (port->clk == NULL || IS_ERR(port->clk)) {
+		port->clk = clk_get(NULL, "uart");
+		if (port->clk == NULL || IS_ERR(port->clk))
+			return -ENODEV;
+		port->port.uartclk = clk_get_rate(port->clk);
+	}
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+	else
+		auart_console_get_options(port, &baud, &parity, &bits);
+	return uart_set_options(port, co, baud, parity, bits, flow);
+}
+
+static struct console auart_console = {
+	.name = "ttySP",
+	.write = auart_console_write,
+	.device = uart_console_device,
+	.setup = auart_console_setup,
+	.flags = CON_PRINTBUFFER,
+	.index = -1,
+	.data = &auart_driver,
+};
 
+#ifdef CONFIG_MXS_EARLY_CONSOLE
+static int __init auart_console_init(void)
+{
+	register_console(&auart_console);
+	return 0;
+}
+
+console_initcall(auart_console_init);
+#endif
+
+#endif
 static struct uart_driver auart_driver = {
 	.owner = THIS_MODULE,
 	.driver_name = "auart",
@@ -769,6 +949,9 @@ static struct uart_driver auart_driver = {
 	.major = MXS_AUART_MAJOR,
 	.minor = 0,
 	.nr = CONFIG_MXS_AUART_PORTS,
+#ifdef CONFIG_SERIAL_MXS_AUART_CONSOLE
+	.cons = &auart_console,
+#endif
 };
 
 static int __devinit mxs_auart_probe(struct platform_device *pdev)
@@ -849,6 +1032,10 @@ static int __devinit mxs_auart_probe(struct platform_device *pdev)
 
 	device_init_wakeup(&pdev->dev, 1);
 
+#ifdef CONFIG_SERIAL_MXS_AUART_CONSOLE
+	memcpy(&auart_port[pdev->id], s, sizeof(struct mxs_auart_port));
+#endif
+
 	ret = uart_add_one_port(&auart_driver, &s->port);
 	if (ret)
 		goto out_free_clk;
diff --git a/drivers/serial/mxs-duart.c b/drivers/serial/mxs-duart.c
index 5d006f380930..171b8628faee 100644
--- a/drivers/serial/mxs-duart.c
+++ b/drivers/serial/mxs-duart.c
@@ -735,9 +735,40 @@ static int __devexit duart_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int duart_suspend(struct platform_device *pdev,
+		pm_message_t state)
+{
+	int ret = 0;
+	if (!duart_port.suspended) {
+		ret = uart_suspend_port(&duart_drv, &duart_port.port);
+		if (!ret)
+			duart_port.suspended = 1;
+	}
+	return ret;
+}
+
+static int duart_resume(struct platform_device *pdev,
+		pm_message_t state)
+{
+	int ret = 0;
+	if (duart_port.suspended) {
+		ret = uart_resume_port(&duart_drv, &duart_port.port);
+		if (!ret)
+			duart_port.suspended = 0;
+	}
+	return ret;
+}
+#else
+#define	duart_suspend	NULL
+#define	duart_resume	NULL
+#endif
+
 static struct platform_driver duart_driver = {
 	.probe = duart_probe,
 	.remove = __devexit_p(duart_remove),
+	.suspend = duart_suspend,
+	.resume = duart_resume,
 	.driver = {
 		   .name = "mxs-duart",
 		   .owner = THIS_MODULE,
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index e16915107c6b..736a12ff9045 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -261,6 +261,11 @@ config SPI_STMP3XXX
 	help
 	  SPI driver for Freescale STMP37xx/378x SoC SSP interface
 
+config SPI_MXS
+	tristate "Freescale MXS SPI/SSP controller"
+	depends on ARCH_MXS && SPI_MASTER
+	help
+	  SPI driver for Freescale MXS SoC SSP interface
 #
 # Add new SPI master controllers in alphabetical order above this line
 #
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index b7cfb6245c0b..b6dbdf064181 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -33,6 +33,7 @@ obj-$(CONFIG_SPI_MXC)			+= mxc_spi.o
 obj-$(CONFIG_SPI_XILINX)		+= xilinx_spi.o
 obj-$(CONFIG_SPI_SH_SCI)		+= spi_sh_sci.o
 obj-$(CONFIG_SPI_STMP3XXX)		+= spi_stmp.o
+obj-$(CONFIG_SPI_MXS)			+= spi_mxs.o
 # 	... add above this line ...
 
 # SPI protocol drivers (device/link on bus)
diff --git a/drivers/spi/mxc_spi.c b/drivers/spi/mxc_spi.c
index cfe40a4526a7..5f4aa2e90392 100644
--- a/drivers/spi/mxc_spi.c
+++ b/drivers/spi/mxc_spi.c
@@ -657,7 +657,7 @@ void mxc_spi_chipselect(struct spi_device *spi, int is_active)
 		if (spi->mode & SPI_CPHA)
 			ctrl_reg |=
 			    spi_ver_def->mode_mask << spi_ver_def->pha_shift;
-		if (!(spi->mode & SPI_CPOL))
+		if (spi->mode & SPI_CPOL)
 			ctrl_reg |=
 			    spi_ver_def->mode_mask << spi_ver_def->
 			    low_pol_shift;
@@ -824,16 +824,23 @@ int mxc_spi_poll_transfer(struct spi_device *spi, struct spi_transfer *t)
 	master_drv_data->transfer.count = t->len;
 	fifo_size = master_drv_data->spi_ver_def->fifo_size;
 
-	count = (t->len > fifo_size) ? fifo_size : t->len;
-	spi_put_tx_data(master_drv_data->base, count, master_drv_data);
+	while (master_drv_data->transfer.count) {
+		count = (master_drv_data->transfer.count > fifo_size) ?
+			fifo_size : master_drv_data->transfer.count;
 
-	while ((((status = __raw_readl(master_drv_data->test_addr)) &
-		 master_drv_data->spi_ver_def->rx_cnt_mask) >> master_drv_data->
-		spi_ver_def->rx_cnt_off) != count);
+		spi_put_tx_data(master_drv_data->base, count, master_drv_data);
 
-	for (i = 0; i < count; i++) {
-		rx_tmp = __raw_readl(master_drv_data->base + MXC_CSPIRXDATA);
-		master_drv_data->transfer.rx_get(master_drv_data, rx_tmp);
+		while ((((status = __raw_readl(master_drv_data->test_addr)) &
+		  	 master_drv_data->spi_ver_def->rx_cnt_mask) >> master_drv_data->
+			spi_ver_def->rx_cnt_off) != count)
+			;
+
+		for (i = 0; i < count; i++) {
+			rx_tmp = __raw_readl(master_drv_data->base + MXC_CSPIRXDATA);
+			master_drv_data->transfer.rx_get(master_drv_data, rx_tmp);
+		}
+
+		master_drv_data->transfer.count -= count;
 	}
 
 	clk_disable(master_drv_data->clk);
@@ -864,8 +871,19 @@ int mxc_spi_transfer(struct spi_device *spi, struct spi_transfer *t)
 	int chipselect_status;
 	u32 fifo_size;
 
+#if defined(CONFIG_MODULE_CCXMX51)
+	/**
+	 * The ConnectCore i.MX51/Wi-i.MX51 use this bus to communicate with
+	 * the pmic, using poll transfers. Because that bus is also used to
+	 * communicate the cpu with other devices, use also poll transfers
+	 * to avoid conflicts.
+	 */
+	if (spi->master->bus_num == 1) {
+		mxc_spi_poll_transfer(spi, t);
+		return t->len;
+	}
+#endif
 	/* Get the master controller driver data from spi device's master */
-
 	master_drv_data = spi_master_get_devdata(spi->master);
 
 	chipselect_status = __raw_readl(MXC_CSPICONFIG +
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
index 17d89a8124ad..da7c984b5641 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/staging/android/binder.c
@@ -43,6 +43,7 @@ static struct proc_dir_entry *binder_proc_dir_entry_proc;
 static struct hlist_head binder_dead_nodes;
 static HLIST_HEAD(binder_deferred_list);
 static DEFINE_MUTEX(binder_deferred_lock);
+static struct workqueue_struct *binder_deferred_workqueue;
 
 static int binder_read_proc_proc(char *page, char **start, off_t off,
 				 int count, int *eof, void *data);
@@ -2986,6 +2987,7 @@ static void binder_deferred_release(struct binder_proc *proc)
 		int i;
 		for (i = 0; i < proc->buffer_size / PAGE_SIZE; i++) {
 			if (proc->pages[i]) {
+				void *page_addr = proc->buffer + i * PAGE_SIZE;
 				if (binder_debug_mask &
 				    BINDER_DEBUG_BUFFER_ALLOC)
 					printk(KERN_INFO
@@ -2993,6 +2995,8 @@ static void binder_deferred_release(struct binder_proc *proc)
 					       "page %d at %p not freed\n",
 					       proc->pid, i,
 					       proc->buffer + i * PAGE_SIZE);
+				unmap_kernel_range((unsigned long)page_addr,
+					PAGE_SIZE);
 				__free_page(proc->pages[i]);
 				page_count++;
 			}
@@ -3062,7 +3066,7 @@ static void binder_defer_work(struct binder_proc *proc, int defer)
 	if (hlist_unhashed(&proc->deferred_work_node)) {
 		hlist_add_head(&proc->deferred_work_node,
 				&binder_deferred_list);
-		schedule_work(&binder_deferred_work);
+		queue_work(binder_deferred_workqueue, &binder_deferred_work);
 	}
 	mutex_unlock(&binder_deferred_lock);
 }
@@ -3690,6 +3694,10 @@ static int __init binder_init(void)
 {
 	int ret;
 
+	binder_deferred_workqueue = create_singlethread_workqueue("binder");
+	if (!binder_deferred_workqueue)
+		return -ENOMEM;
+
 	binder_proc_dir_entry_root = proc_mkdir("binder", NULL);
 	if (binder_proc_dir_entry_root)
 		binder_proc_dir_entry_proc = proc_mkdir("proc",
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index 6c10b456c6cc..7f64f8fb26b0 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -556,6 +556,7 @@ static struct logger_log VAR = { \
 DEFINE_LOGGER_DEVICE(log_main, LOGGER_LOG_MAIN, 64*1024)
 DEFINE_LOGGER_DEVICE(log_events, LOGGER_LOG_EVENTS, 256*1024)
 DEFINE_LOGGER_DEVICE(log_radio, LOGGER_LOG_RADIO, 64*1024)
+DEFINE_LOGGER_DEVICE(log_system, LOGGER_LOG_SYSTEM, 64*1024)
 
 static struct logger_log *get_log_from_minor(int minor)
 {
@@ -565,6 +566,8 @@ static struct logger_log *get_log_from_minor(int minor)
 		return &log_events;
 	if (log_radio.misc.minor == minor)
 		return &log_radio;
+	if (log_system.misc.minor == minor)
+		return &log_system;
 	return NULL;
 }
 
@@ -601,6 +604,10 @@ static int __init logger_init(void)
 	if (unlikely(ret))
 		goto out;
 
+	ret = init_log(&log_system);
+	if (unlikely(ret))
+		goto out;
+
 out:
 	return ret;
 }
diff --git a/drivers/staging/android/logger.h b/drivers/staging/android/logger.h
index a562434d7419..2cb06e9d8f98 100644
--- a/drivers/staging/android/logger.h
+++ b/drivers/staging/android/logger.h
@@ -32,6 +32,7 @@ struct logger_entry {
 
 #define LOGGER_LOG_RADIO	"log_radio"	/* radio-related messages */
 #define LOGGER_LOG_EVENTS	"log_events"	/* system/hardware events */
+#define LOGGER_LOG_SYSTEM	"log_system"	/* system/framework messages */
 #define LOGGER_LOG_MAIN		"log_main"	/* everything else */
 
 #define LOGGER_ENTRY_MAX_LEN		(4*1024)
diff --git a/drivers/staging/android/lowmemorykiller.c b/drivers/staging/android/lowmemorykiller.c
index 803b891dc85e..572dc062759a 100644
--- a/drivers/staging/android/lowmemorykiller.c
+++ b/drivers/staging/android/lowmemorykiller.c
@@ -18,6 +18,9 @@
 #include <linux/mm.h>
 #include <linux/oom.h>
 #include <linux/sched.h>
+#include <linux/nodemask.h>
+#include <linux/vmstat.h>
+#include <linux/notifier.h>
 
 static int lowmem_shrink(int nr_to_scan, gfp_t gfp_mask);
 
@@ -41,6 +44,8 @@ static size_t lowmem_minfree[6] = {
 };
 static int lowmem_minfree_size = 4;
 
+static struct task_struct *lowmem_deathpending;
+
 #define lowmem_print(level, x...)			\
 	do {						\
 		if (lowmem_debug_level >= (level))	\
@@ -54,6 +59,24 @@ module_param_array_named(minfree, lowmem_minfree, uint, &lowmem_minfree_size,
 			 S_IRUGO | S_IWUSR);
 module_param_named(debug_level, lowmem_debug_level, uint, S_IRUGO | S_IWUSR);
 
+static int
+task_notify_func(struct notifier_block *self, unsigned long val, void *data);
+
+static struct notifier_block task_nb = {
+	.notifier_call  = task_notify_func,
+};
+
+static int
+task_notify_func(struct notifier_block *self, unsigned long val, void *data)
+{
+	struct task_struct *task = data;
+	if (task == lowmem_deathpending) {
+		lowmem_deathpending = NULL;
+		task_free_unregister(&task_nb);
+	}
+	return NOTIFY_OK;
+}
+
 static int lowmem_shrink(int nr_to_scan, gfp_t gfp_mask)
 {
 	struct task_struct *p;
@@ -67,6 +90,24 @@ static int lowmem_shrink(int nr_to_scan, gfp_t gfp_mask)
 	int array_size = ARRAY_SIZE(lowmem_adj);
 	int other_free = global_page_state(NR_FREE_PAGES);
 	int other_file = global_page_state(NR_FILE_PAGES);
+	int node;
+
+	/*
+	* If we already have a death outstanding, then
+	* bail out right away; indicating to vmscan
+	* that we have nothing further to offer on
+	* this pass.
+	*/
+	if (lowmem_deathpending)
+		return 0;
+
+	for_each_node_state(node, N_HIGH_MEMORY) {
+		struct zone *z =
+			&NODE_DATA(node)->node_zones[ZONE_DMA];
+
+		other_free -= zone_page_state(z, NR_FREE_PAGES);
+		other_file -= zone_page_state(z, NR_FILE_PAGES);
+	}
 
 	if (lowmem_adj_size < array_size)
 		array_size = lowmem_adj_size;
@@ -128,9 +169,19 @@ static int lowmem_shrink(int nr_to_scan, gfp_t gfp_mask)
 			     p->pid, p->comm, oom_adj, tasksize);
 	}
 	if (selected) {
+		if (fatal_signal_pending(selected)) {
+			pr_warning("process %d is suffering a slow death\n",
+				   selected->pid);
+			read_unlock(&tasklist_lock);
+			return rem;
+		}
 		lowmem_print(1, "send sigkill to %d (%s), adj %d, size %d\n",
 			     selected->pid, selected->comm,
 			     selected_oom_adj, selected_tasksize);
+
+		lowmem_deathpending = selected;
+		task_free_register(&task_nb);
+
 		force_sig(SIGKILL, selected);
 		rem -= selected_tasksize;
 	}
diff --git a/drivers/uio/uio_pdrv_genirq.c b/drivers/uio/uio_pdrv_genirq.c
index 3f06818cf9fa..31fcd3270b26 100644
--- a/drivers/uio/uio_pdrv_genirq.c
+++ b/drivers/uio/uio_pdrv_genirq.c
@@ -157,6 +157,12 @@ static int uio_pdrv_genirq_remove(struct platform_device *pdev)
 	struct uio_pdrv_genirq_platdata *priv = platform_get_drvdata(pdev);
 
 	uio_unregister_device(priv->uioinfo);
+
+	priv->uioinfo->irq_flags = 0;
+	priv->uioinfo->handler = NULL;
+	priv->uioinfo->irqcontrol = NULL;
+	priv->uioinfo->priv = NULL;
+
 	kfree(priv);
 	return 0;
 }
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 2bfc41ece0e1..b8134ad5f05f 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -59,6 +59,7 @@
 #include <linux/init.h>
 #include <linux/slab.h>
 #include <linux/tty.h>
+#include <linux/serial.h>
 #include <linux/tty_driver.h>
 #include <linux/tty_flip.h>
 #include <linux/module.h>
@@ -609,6 +610,7 @@ static int acm_tty_open(struct tty_struct *tty, struct file *filp)
 	acm->throttle = 0;
 
 	tasklet_schedule(&acm->urb_task);
+	set_bit(ASYNCB_INITIALIZED, &acm->port.flags);
 	rv = tty_port_block_til_ready(&acm->port, tty, filp);
 done:
 	mutex_unlock(&acm->mutex);
diff --git a/drivers/usb/core/driver.c b/drivers/usb/core/driver.c
index 69e5773abfce..07f503aa9078 100644
--- a/drivers/usb/core/driver.c
+++ b/drivers/usb/core/driver.c
@@ -1754,6 +1754,9 @@ int usb_resume(struct device *dev, pm_message_t msg)
 
 	udev = to_usb_device(dev);
 
+/* At otg mode, if it is a device wakeup interrupt, the host should do nothing */
+	if (udev->bus->is_b_host)
+		return 0;
 	/* If udev->skip_sys_resume is set then udev was already suspended
 	 * when the system sleep started, so we don't want to resume it
 	 * during this system wakeup.
@@ -1765,7 +1768,7 @@ int usb_resume(struct device *dev, pm_message_t msg)
 	/* Avoid PM error messages for devices disconnected while suspended
 	 * as we'll display regular disconnect messages just a bit later.
 	 */
-	if (status == -ENODEV)
+	if (status == -ENODEV || status == -ESHUTDOWN)
 		return 0;
 	return status;
 }
diff --git a/drivers/usb/core/generic.c b/drivers/usb/core/generic.c
index 66e8a424c9f4..539a2c0dde00 100644
--- a/drivers/usb/core/generic.c
+++ b/drivers/usb/core/generic.c
@@ -196,7 +196,6 @@ extern void usb_host_set_wakeup(struct device *wkup_dev, bool para);
 static int generic_suspend(struct usb_device *udev, pm_message_t msg)
 {
 	int rc;
-	u32 temp;
 
 	/* Normal USB devices suspend through their upstream port.
 	 * Root hubs don't have upstream ports to suspend,
@@ -204,25 +203,7 @@ static int generic_suspend(struct usb_device *udev, pm_message_t msg)
 	 * interfaces manually by doing a bus (or "global") suspend.
 	 */
 	if (!udev->parent) {
-		struct usb_hcd  *hcd =
-			container_of(udev->bus, struct usb_hcd, self);
-		struct fsl_usb2_platform_data *pdata;
-		pdata = hcd->self.controller->platform_data;
-
 		rc = hcd_bus_suspend(udev, msg);
-
-		if (device_may_wakeup(hcd->self.controller)) {
-			clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-			/* enable remote wake up irq */
-			usb_host_set_wakeup(hcd->self.controller, true);
-
-			/* Put PHY into low power mode */
-			temp = readl(hcd->regs + 0x184);
-			writel(temp | (1 << 23), (hcd->regs + 0x184));
-
-			if (pdata->usb_clock_for_pm)
-				pdata->usb_clock_for_pm(false);
-		}
 	/* Non-root devices don't need to do anything for FREEZE or PRETHAW */
 	} else if (msg.event == PM_EVENT_FREEZE ||
 			msg.event == PM_EVENT_PRETHAW)
@@ -236,7 +217,6 @@ static int generic_suspend(struct usb_device *udev, pm_message_t msg)
 static int generic_resume(struct usb_device *udev, pm_message_t msg)
 {
 	int rc;
-	u32 temp;
 
 	/* Normal USB devices resume/reset through their upstream port.
 	 * Root hubs don't have upstream ports to resume or reset,
@@ -244,13 +224,6 @@ static int generic_resume(struct usb_device *udev, pm_message_t msg)
 	 * interfaces manually by doing a bus (or "global") resume.
 	 */
 	if (!udev->parent) {
-		struct usb_hcd  *hcd =
-			container_of(udev->bus, struct usb_hcd, self);
-
-		if (device_may_wakeup(hcd->self.controller)) {
-			temp = readl(hcd->regs + 0x184);
-			writel(temp & (~(1 << 23)), (hcd->regs + 0x184));
-		}
 		rc = hcd_bus_resume(udev, msg);
 	} else
 		rc = usb_port_resume(udev, msg);
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index d27ad104731c..2f47bdc7c93a 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -1739,6 +1739,7 @@ int hcd_bus_suspend(struct usb_device *rhdev, pm_message_t msg)
 	int		status;
 	int		old_state = hcd->state;
 
+	printk("%s\n", __func__);
 	dev_dbg(&rhdev->dev, "bus %s%s\n",
 			(msg.event & PM_EVENT_AUTO ? "auto-" : ""), "suspend");
 	if (!hcd->driver->bus_suspend) {
@@ -1876,7 +1877,6 @@ EXPORT_SYMBOL_GPL(usb_bus_start_enum);
 irqreturn_t usb_hcd_irq (int irq, void *__hcd)
 {
 	struct usb_hcd		*hcd = __hcd;
-	struct fsl_usb2_platform_data *pdata;
 	unsigned long		flags;
 	irqreturn_t		rc;
 
@@ -1885,25 +1885,14 @@ irqreturn_t usb_hcd_irq (int irq, void *__hcd)
 	 * assume it's never used.
 	 */
 	local_irq_save(flags);
-
-	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
-		/* Need open clock for register access */
-		pdata = hcd->self.controller->platform_data;
-		if (pdata->usb_clock_for_pm)
-			pdata->usb_clock_for_pm(true);
-
-		/* if receive a remote wakeup interrrupt after suspend */
-		if (usb_host_wakeup_irq(hcd->self.controller)) {
-			/* disable remote wake up irq */
-			usb_host_set_wakeup(hcd->self.controller, false);
-
-			set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-			hcd->driver->irq(hcd);
-			rc = IRQ_HANDLED;
-		} else
+	/* At otg mode, the host does need to handle device interrupt */
+	if (hcd->self.is_b_host){
+		local_irq_restore(flags);
+		return IRQ_NONE;
+	}
+	else if (unlikely(hcd->state == HC_STATE_HALT ||
+		!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags))) {
 			rc = IRQ_NONE;
-	} else if (unlikely(hcd->state == HC_STATE_HALT)) {
-		rc = IRQ_NONE;
 	} else if (hcd->driver->irq(hcd) == IRQ_NONE) {
 		rc = IRQ_NONE;
 	} else {
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index d47201c75915..cc8f911afbc4 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -1177,12 +1177,6 @@ static int hub_probe(struct usb_interface *intf, const struct usb_device_id *id)
 			"Unsupported bus topology: hub nested too deep\n");
 		return -E2BIG;
 	}
-#ifdef CONFIG_PM
-	/* Defaultly disable autosuspend for hub and reley on sys
-	 * to enable it.
-	 */
-	hdev->autosuspend_disabled = 1;
-#endif
 
 #ifdef	CONFIG_USB_OTG_BLACKLIST_HUB
 	if (hdev->parent) {
@@ -2304,7 +2298,6 @@ static int hub_suspend(struct usb_interface *intf, pm_message_t msg)
 	struct usb_hub		*hub = usb_get_intfdata (intf);
 	struct usb_device	*hdev = hub->hdev;
 	unsigned		port1;
-
 	/* fail if children aren't already suspended */
 	for (port1 = 1; port1 <= hdev->maxchild; port1++) {
 		struct usb_device	*udev;
@@ -2328,8 +2321,15 @@ static int hub_suspend(struct usb_interface *intf, pm_message_t msg)
 static int hub_resume(struct usb_interface *intf)
 {
 	struct usb_hub *hub = usb_get_intfdata(intf);
+	struct usb_hcd *hcd = bus_to_hcd(hub->hdev->bus);
 
 	dev_dbg(&intf->dev, "%s\n", __func__);
+	/* At otg mode, if the hcd which the hub is attached to is not accessible,
+	 * It should do nothing.
+	 */
+	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags))
+		return 0;
+
 	hub_activate(hub, HUB_RESUME);
 	return 0;
 }
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index ce44379bd172..c29ebda61b2f 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -466,7 +466,6 @@ config USB_GOKU
 config USB_GADGET_ARC
 	boolean "Freescale USB Device Controller"
 	depends on ARCH_MXC || ARCH_STMP3XXX || ARCH_MXS
-	depends on !USB_EHCI_ARC_OTG
 	select USB_GADGET_DUALSPEED
 	select USB_OTG_UTILS
 	select USB_GADGET_DUALSPEED if USB_GADGET_FSL_1504 || USB_GADGET_FSL_UTMI
@@ -792,6 +791,15 @@ config USB_G_PRINTER
 	  For more information, see Documentation/usb/gadget_printer.txt
 	  which includes sample code for accessing the device file.
 
+config USB_ANDROID
+	tristate "Android Gadget"
+	depends on SWITCH
+	help
+	  The Android gadget provides mass storage and adb transport.
+
+	  Say "y" to link the driver statically, or "m" to build a
+	  dynamically linked module called "g_android".
+
 config USB_CDC_COMPOSITE
 	tristate "CDC Composite Device (Ethernet and ACM)"
 	depends on NET
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 477114e43372..545c0e256e28 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -41,6 +41,7 @@ gadgetfs-objs			:= inode.o
 g_file_storage-objs		:= file_storage.o
 g_printer-objs			:= printer.o
 g_cdc-objs			:= cdc2.o
+g_android-objs			:= android.o f_adb.o f_mass_storage.o
 
 obj-$(CONFIG_USB_ZERO)		+= g_zero.o
 obj-$(CONFIG_USB_AUDIO)		+= g_audio.o
@@ -51,4 +52,5 @@ obj-$(CONFIG_USB_G_SERIAL)	+= g_serial.o
 obj-$(CONFIG_USB_G_PRINTER)	+= g_printer.o
 obj-$(CONFIG_USB_MIDI_GADGET)	+= g_midi.o
 obj-$(CONFIG_USB_CDC_COMPOSITE) += g_cdc.o
+obj-$(CONFIG_USB_ANDROID)	+= g_android.o
 
diff --git a/drivers/usb/gadget/arcotg_udc.c b/drivers/usb/gadget/arcotg_udc.c
index 1577c93c35bb..8e94549f891e 100644
--- a/drivers/usb/gadget/arcotg_udc.c
+++ b/drivers/usb/gadget/arcotg_udc.c
@@ -106,19 +106,6 @@ extern struct resource *otg_get_resources(void);
 
 extern void fsl_platform_set_test_mode(struct fsl_usb2_platform_data *pdata, enum usb_test_mode mode);
 
-static inline void
-dr_wake_up_enable(struct fsl_udc *udc, bool enable)
-{
-	struct fsl_usb2_platform_data *pdata;
-	pdata = udc->pdata;
-
-	if (enable && (!device_may_wakeup(udc_controller->gadget.dev.parent)))
-		return;
-
-	if (pdata->wake_up_enable)
-		pdata->wake_up_enable(pdata, enable);
-}
-
 #ifdef CONFIG_WORKAROUND_ARCUSB_REG_RW
 static void safe_writel(u32 val32, void *addr)
 {
@@ -177,6 +164,25 @@ static inline void dump_ep_queue(struct fsl_ep *ep)
 }
 #endif
 
+#if (defined CONFIG_ARCH_MX35 || defined CONFIG_ARCH_MX25)
+/*
+ * The Phy at MX35 and MX25 have bugs, it must disable, and re-eable phy
+ * if the phy clock is disabled before
+ */
+static void reset_phy(void)
+{
+	u32 phyctrl;
+	phyctrl = fsl_readl(&dr_regs->phyctrl1);
+	phyctrl &= ~PHY_CTRL0_USBEN;
+	fsl_writel(phyctrl, &dr_regs->phyctrl1);
+
+	phyctrl = fsl_readl(&dr_regs->phyctrl1);
+	phyctrl |= PHY_CTRL0_USBEN;
+	fsl_writel(phyctrl, &dr_regs->phyctrl1);
+}
+#else
+static void reset_phy(void){; }
+#endif
 /*-----------------------------------------------------------------
  * done() - retire a request; caller blocked irqs
  * @status : request status to be set, only works when
@@ -264,9 +270,12 @@ static void done(struct fsl_ep *ep, struct fsl_req *req, int status)
 static void nuke(struct fsl_ep *ep, int status)
 {
 	ep->stopped = 1;
-
-	/* Flush fifo */
-	fsl_ep_fifo_flush(&ep->ep);
+	/*
+	 * At udc stop mode, the clock is already off
+	 * So flush fifo, should be done at clock on mode.
+	 */
+	if (!ep->udc->stopped)
+		fsl_ep_fifo_flush(&ep->ep);
 
 	/* Whether this eq has request linked */
 	while (!list_empty(&ep->queue)) {
@@ -281,31 +290,85 @@ static void nuke(struct fsl_ep *ep, int status)
 /*------------------------------------------------------------------
 	Internal Hardware related function
  ------------------------------------------------------------------*/
+static inline void
+dr_wake_up_enable(struct fsl_udc *udc, bool enable)
+{
+	struct fsl_usb2_platform_data *pdata;
+	pdata = udc->pdata;
 
-static void dr_phy_low_power_mode(struct fsl_udc *udc, bool enable)
+	if (pdata && pdata->wake_up_enable)
+		pdata->wake_up_enable(pdata, enable);
+}
+static bool clk_stoped = false;
+static inline void dr_clk_gate(bool on)
 {
-	u32 temp;
+	struct fsl_usb2_platform_data *pdata = udc_controller->pdata;
 
-	if (!device_may_wakeup(udc_controller->gadget.dev.parent))
+	if (!pdata || !pdata->usb_clock_for_pm)
 		return;
+	if (on && clk_stoped) {
+		pdata->usb_clock_for_pm(true);
+		clk_stoped = false;
+	}
+	if (!on && !clk_stoped) {
+		pdata->usb_clock_for_pm(false);
+		clk_stoped = true;
+	}
+	if (on)
+		reset_phy();
+}
 
-	if (enable) {
-		temp = fsl_readl(&dr_regs->portsc1);
-		temp |= PORTSCX_PHY_LOW_POWER_SPD;
-		fsl_writel(temp, &dr_regs->portsc1);
+static void dr_phy_low_power_mode(struct fsl_udc *udc, bool enable)
+{
+	struct fsl_usb2_platform_data *pdata = udc->pdata;
+	u32 portsc;
 
-		if (udc_controller->pdata->usb_clock_for_pm)
-			udc_controller->pdata->usb_clock_for_pm(false);
+	if (pdata && pdata->phy_lowpower_suspend) {
+		pdata->phy_lowpower_suspend(enable);
 	} else {
-		if (udc_controller->pdata->usb_clock_for_pm)
-			udc_controller->pdata->usb_clock_for_pm(true);
-
-		temp = fsl_readl(&dr_regs->portsc1);
-		temp &= ~PORTSCX_PHY_LOW_POWER_SPD;
-		fsl_writel(temp, &dr_regs->portsc1);
+		if (enable){
+			portsc = fsl_readl(&dr_regs->portsc1);
+			portsc |= PORTSCX_PHY_LOW_POWER_SPD;
+			fsl_writel(portsc, &dr_regs->portsc1);
+		} else {
+			portsc = fsl_readl(&dr_regs->portsc1);
+			portsc &= ~PORTSCX_PHY_LOW_POWER_SPD;
+			fsl_writel(portsc, &dr_regs->portsc1);
+		}
 	}
 }
 
+
+/* workaroud for some boards, maybe there is a large capacitor between the ground and the Vbus
+ * that will cause the vbus dropping very slowly when device is detached,
+ * may cost 2-3 seconds to below 0.8V */
+static void udc_wait_b_session_low(void)
+{
+	u32 temp;
+	u32 wait = 5000; /* max wait time is 5000 ms */
+	/* if we are in host mode, don't need to care the B session */
+	if ((fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID) == 0)
+		return;
+	/* if the udc is dettached , there will be a suspend irq */
+	if (udc_controller->usb_state != USB_STATE_SUSPENDED)
+		return;
+	temp = fsl_readl(&dr_regs->otgsc);
+	temp &= ~(OTGSC_B_SESSION_VALID_IRQ_EN );
+	fsl_writel(temp, &dr_regs->otgsc);
+
+	do {
+		if (!(fsl_readl(&dr_regs->otgsc) & OTGSC_B_SESSION_VALID))
+			break;
+		mdelay(1);
+		wait -= 1;
+	} while(wait);
+	if (!wait)
+		printk("ERROR!!!!!: the vbus can not be lower then 0.8V for 5 seconds, Pls Check your HW design\n");
+	temp = fsl_readl(&dr_regs->otgsc);
+	temp |= (OTGSC_B_SESSION_VALID_IRQ_EN );
+	fsl_writel(temp, &dr_regs->otgsc);
+}
+
 static int dr_controller_setup(struct fsl_udc *udc)
 {
 	unsigned int tmp = 0, portctrl = 0;
@@ -427,37 +490,36 @@ static void dr_controller_run(struct fsl_udc *udc)
 
 	fsl_writel(temp, &dr_regs->usbintr);
 
-	if (device_may_wakeup(udc_controller->gadget.dev.parent)) {
-		/* enable BSV irq */
-		temp = fsl_readl(&dr_regs->otgsc);
-		temp |= OTGSC_B_SESSION_VALID_IRQ_EN;
-		fsl_writel(temp, &dr_regs->otgsc);
-	}
+	/* enable BSV irq */
+	temp = fsl_readl(&dr_regs->otgsc);
+	temp |= OTGSC_B_SESSION_VALID_IRQ_EN;
+	fsl_writel(temp, &dr_regs->otgsc);
 
 	/* If vbus not on and used low power mode */
-	if (!(fsl_readl(&dr_regs->otgsc) & OTGSC_B_SESSION_VALID)
-	    && device_may_wakeup(udc_controller->gadget.dev.parent)) {
-		/* enable wake up */
-		dr_wake_up_enable(udc, true);
+	if (!(temp & OTGSC_B_SESSION_VALID)) {
 		/* Set stopped before low power mode */
 		udc->stopped = 1;
-		/* close PHY clock */
+		/* enable wake up */
+		dr_wake_up_enable(udc, true);
+		/* enter lower power mode */
 		dr_phy_low_power_mode(udc, true);
-		printk(KERN_INFO "udc enter low power mode \n");
+		printk(KERN_INFO "%s: udc enter low power mode \n", __func__);
 	} else {
+#ifdef CONFIG_ARCH_MX37
 		/*
 		 add some delay for USB timing issue. USB may be
 		 recognize as FS device
 		 during USB gadget remote wake up function
 		*/
 		mdelay(100);
+#endif
 		/* Clear stopped bit */
 		udc->stopped = 0;
-		/* Set controller to Run */
+
+		/* The usb line has already been connected to pc */
 		temp = fsl_readl(&dr_regs->usbcmd);
 		temp |= USB_CMD_RUN_STOP;
 		fsl_writel(temp, &dr_regs->usbcmd);
-		printk(KERN_INFO "udc run \n");
 	}
 
 	return;
@@ -680,7 +742,7 @@ static int fsl_ep_enable(struct usb_ep *_ep,
 	case USB_ENDPOINT_XFER_ISOC:
 		/* Calculate transactions needed for high bandwidth iso */
 		mult = (unsigned char)(1 + ((max >> 11) & 0x03));
-		max = max & 0x8ff;	/* bit 0~10 */
+		max = max & 0x7ff;	/* bit 0~10 */
 		/* 3 transactions at most */
 		if (mult > 3)
 			goto en_done;
@@ -1953,15 +2015,36 @@ static void suspend_irq(struct fsl_udc *udc)
 		udc->driver->suspend(&udc->gadget);
 }
 
-/* Process Wake up interrupt */
-static void wake_up_irq(struct fsl_udc *udc)
-{
-	pr_debug("%s\n", __func__);
-
-	/* disable wake up irq */
-	dr_wake_up_enable(udc_controller, false);
-
-	udc->stopped = 0;
+/* Process Wake up interrupt
+ * Be careful that some boards will use ID pin to control the VBUS on/off
+ * in these case, after the device enter the lowpower mode(clk off,
+ * phy lowpower mode, wakeup enable), then an udisk is attaced to the otg port,
+ * there will be an Vbus wakeup event and then an ID change wakeup, But the Vbus
+ * event is not expected, so there is an workaround that will detect the ID, if ID=0
+ * we just need the ID event so we can not disable the wakeup
+ *
+ * false: host wakeup event
+ * true: device wakeup event
+*/
+static bool wake_up_irq(struct fsl_udc *udc)
+{
+	/* Because the IC design needs to remove the glitch on ID so the otgsc bit 8 will
+	* be delayed max 2 ms to show the real ID pin value
+	*/
+	mdelay(3);
+
+	/* if the ID=0, let arc host process the wakeup */
+	if (fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID) {
+		dr_wake_up_enable(udc_controller, false);
+		dr_phy_low_power_mode(udc, false);
+		printk("device wake up event\n");
+		return true;
+	}else {/* wakeup is vbus wake event, but not for device so we need to clear b session */
+		int irq_src = fsl_readl(&dr_regs->otgsc) & (~OTGSC_ID_CHANGE_IRQ_STS);
+		fsl_writel(irq_src, &dr_regs->otgsc);
+		printk("The host wakeup event, should be handled by host\n");
+		return false;
+	}
 }
 
 static void bus_resume(struct fsl_udc *udc)
@@ -2018,72 +2101,60 @@ static void reset_irq(struct fsl_udc *udc)
 	/* Write 1s to the flush register */
 	fsl_writel(0xffffffff, &dr_regs->endptflush);
 
-	if (fsl_readl(&dr_regs->portsc1) & PORTSCX_PORT_RESET) {
-		VDBG("Bus reset");
-		/* Bus is reseting */
-		udc->bus_reset = 1;
-		/* Reset all the queues, include XD, dTD, EP queue
-		 * head and TR Queue */
-		reset_queues(udc);
-		udc->usb_state = USB_STATE_DEFAULT;
-	} else {
-		VDBG("Controller reset");
-		/* initialize usb hw reg except for regs for EP, not
-		 * touch usbintr reg */
-		dr_controller_setup(udc);
-
-		/* Reset all internal used Queues */
-		reset_queues(udc);
-
-		ep0_setup(udc);
-
-		/* Enable DR IRQ reg, Set Run bit, change udc state */
-		dr_controller_run(udc);
-		udc->usb_state = USB_STATE_ATTACHED;
-	}
+	/* Bus is reseting */
+	udc->bus_reset = 1;
+	/* Reset all the queues, include XD, dTD, EP queue
+	 * head and TR Queue */
+	reset_queues(udc);
+	udc->usb_state = USB_STATE_DEFAULT;
 }
 
 /* if wakup udc, return true; else return false*/
 bool try_wake_up_udc(struct fsl_udc *udc)
 {
 	u32 irq_src;
+	bool b_device;
 
 	/* when udc is stopped, only handle wake up irq */
 	if (udc->stopped) {
-		if (!device_may_wakeup(&(udc->pdata->pdev->dev)))
-			return false;
-
-		dr_phy_low_power_mode(udc_controller, false);
-
 		/* check to see if wake up irq */
 		irq_src =  fsl_readl(&dr_regs->usbctrl);
 		if (irq_src & USB_CTRL_OTG_WUIR) {
-			wake_up_irq(udc);
-		} else {
-			dr_phy_low_power_mode(udc_controller, true);
+			if (wake_up_irq(udc) == false){
+				return false; /* host wakeup event */
+			}
 		}
 	}
 
-	if (!device_may_wakeup(udc_controller->gadget.dev.parent))
-		return true;
-
 	/* check if Vbus change irq */
 	irq_src = fsl_readl(&dr_regs->otgsc);
 	if (irq_src & OTGSC_B_SESSION_VALID_IRQ_STS) {
 		u32 tmp;
+	/* Because the IC design needs to remove the glitch on ID so the otgsc bit 8 will
+	 * be delayed max 2 ms to show the real ID pin value, as it needs to use ID to judge
+	 * host or device
+	 */
+		mdelay(3);
+		b_device = (irq_src & OTGSC_STS_USB_ID)? true:false;
 		fsl_writel(irq_src, &dr_regs->otgsc);
+		if (!b_device)
+			return false;
 		tmp = fsl_readl(&dr_regs->usbcmd);
 		/* check BSV bit to see if fall or rise */
 		if (irq_src & OTGSC_B_SESSION_VALID) {
+			if (udc->suspended) /*let the system pm resume the udc */
+				return true;
 			udc->stopped = 0;
 			fsl_writel(tmp | USB_CMD_RUN_STOP, &dr_regs->usbcmd);
-			printk(KERN_INFO "udc out low power mode\n");
+			printk(KERN_INFO "%s: udc out low power mode\n", __func__);
 		} else {
-			printk(KERN_INFO "udc enter low power mode \n");
+			printk(KERN_INFO "%s: udc enter low power mode \n", __func__);
+			if (udc->driver)
+				udc->driver->disconnect(&udc->gadget);
 			fsl_writel(tmp & ~USB_CMD_RUN_STOP, &dr_regs->usbcmd);
+			udc->stopped = 1;
 			/* enable wake up */
 			dr_wake_up_enable(udc, true);
-			udc->stopped = 1;
 			/* close USB PHY clock */
 			dr_phy_low_power_mode(udc, true);
 			return false;
@@ -2092,7 +2163,6 @@ bool try_wake_up_udc(struct fsl_udc *udc)
 
 	return true;
 }
-
 /*
  * USB device controller interrupt handler
  */
@@ -2103,15 +2173,29 @@ static irqreturn_t fsl_udc_irq(int irq, void *_udc)
 	irqreturn_t status = IRQ_NONE;
 	unsigned long flags;
 
-	if (try_wake_up_udc(udc) == false)
-		return IRQ_NONE;
-
 	spin_lock_irqsave(&udc->lock, flags);
+	if (udc->stopped)
+		dr_clk_gate(true);
+
+	if (try_wake_up_udc(udc) == false) {
+		goto irq_end;
+	}
+#ifdef CONFIG_USB_OTG
+	/* if no gadget register in this driver, we need do noting */
+	if (udc->transceiver->gadget == NULL)
+		goto irq_end;
+
+	/* only handle device interrupt event */
+	if (!(fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID)) {
+		goto irq_end;
+	}
+#endif
+
 	irq_src = fsl_readl(&dr_regs->usbsts) & fsl_readl(&dr_regs->usbintr);
 	/* Clear notification bits */
 	fsl_writel(irq_src, &dr_regs->usbsts);
 
-	/* VDBG("irq_src [0x%8x]", irq_src); */
+	VDBG("0x%x\n", irq_src);
 
 	/* Need to resume? */
 	if (udc->usb_state == USB_STATE_SUSPENDED)
@@ -2156,12 +2240,24 @@ static irqreturn_t fsl_udc_irq(int irq, void *_udc)
 
 	/* Sleep Enable (Suspend) */
 	if (irq_src & USB_STS_SUSPEND) {
+		VDBG("suspend int");
 		suspend_irq(udc);
 		status = IRQ_HANDLED;
 	}
 
 	if (irq_src & (USB_STS_ERR | USB_STS_SYS_ERR)) {
-		VDBG("Error IRQ %x ", irq_src);
+		printk(KERN_ERR "Error IRQ %x ", irq_src);
+		if (irq_src & USB_STS_SYS_ERR) {
+			printk(KERN_ERR "This error can't be recoveried, \
+					please reboot your board\n");
+			printk(KERN_ERR "If this error happens frequently, \
+				please check your dma buffer\n");
+		}
+	}
+
+irq_end:
+	if (udc->stopped){
+		dr_clk_gate(false);
 	}
 
 	spin_unlock_irqrestore(&udc->lock, flags);
@@ -2176,9 +2272,6 @@ int usb_gadget_register_driver(struct usb_gadget_driver *driver)
 {
 	int retval = -ENODEV;
 	unsigned long flags = 0;
-#ifndef CONFIG_USB_OTG
-	u32 portsc;
-#endif
 
 	if (!udc_controller)
 		return -ENODEV;
@@ -2196,18 +2289,19 @@ int usb_gadget_register_driver(struct usb_gadget_driver *driver)
 	spin_lock_irqsave(&udc_controller->lock, flags);
 
 	driver->driver.bus = 0;
+	udc_controller->pdata->port_enables = 1;
 	/* hook up the driver */
 	udc_controller->driver = driver;
 	udc_controller->gadget.dev.driver = &driver->driver;
 	spin_unlock_irqrestore(&udc_controller->lock, flags);
-#ifndef CONFIG_USB_OTG
-	if (udc_controller->pdata->usb_clock_for_pm)
-		udc_controller->pdata->usb_clock_for_pm(true);
+	dr_clk_gate(true);
+	/* It doesn't need to switch usb from low power mode to normal mode
+	 * at otg mode
+	 */
+	if (!udc_controller->transceiver){
+		dr_phy_low_power_mode(udc_controller, false);
+	}
 
-	portsc = fsl_readl(&dr_regs->portsc1);
-	portsc &= ~PORTSCX_PHY_LOW_POWER_SPD;
-	fsl_writel(portsc, &dr_regs->portsc1);
-#endif
 	/* bind udc driver to gadget driver */
 	retval = driver->bind(&udc_controller->gadget);
 	if (retval) {
@@ -2219,30 +2313,30 @@ int usb_gadget_register_driver(struct usb_gadget_driver *driver)
 
 	if (udc_controller->transceiver) {
 		/* Suspend the controller until OTG enable it */
-		udc_controller->stopped = 1;
+		udc_controller->suspended = 1;/* let the otg resume it */
 		printk(KERN_INFO "Suspend udc for OTG auto detect\n");
 		dr_wake_up_enable(udc_controller, true);
-		dr_phy_low_power_mode(udc_controller, true);
 
 		/* export udc suspend/resume call to OTG */
 		udc_controller->gadget.dev.driver->suspend = (dev_sus)fsl_udc_suspend;
 		udc_controller->gadget.dev.driver->resume = (dev_res)fsl_udc_resume;
 
 		/* connect to bus through transceiver */
-		if (udc_controller->transceiver) {
-			retval = otg_set_peripheral(udc_controller->transceiver,
-						    &udc_controller->gadget);
-			if (retval < 0) {
-				ERR("can't bind to transceiver\n");
-				driver->unbind(&udc_controller->gadget);
-				udc_controller->gadget.dev.driver = 0;
-				udc_controller->driver = 0;
-				return retval;
-			}
+		retval = otg_set_peripheral(udc_controller->transceiver,
+					    &udc_controller->gadget);
+		if (retval < 0) {
+			ERR("can't bind to transceiver\n");
+			driver->unbind(&udc_controller->gadget);
+			udc_controller->gadget.dev.driver = 0;
+			udc_controller->driver = 0;
+			return retval;
 		}
+		//dr_clk_gate(false);
 	} else {
 		/* Enable DR IRQ reg and Set usbcmd reg  Run bit */
 		dr_controller_run(udc_controller);
+		if (udc_controller->stopped)
+			dr_clk_gate(false);
 		udc_controller->usb_state = USB_STATE_ATTACHED;
 		udc_controller->ep0_dir = 0;
 	}
@@ -2250,8 +2344,10 @@ int usb_gadget_register_driver(struct usb_gadget_driver *driver)
 			udc_controller->gadget.name, driver->driver.name);
 
 out:
-	if (retval)
+	if (retval){
 		printk(KERN_DEBUG "retval %d \n", retval);
+		udc_controller->pdata->port_enables = 0;
+	}
 	return retval;
 }
 EXPORT_SYMBOL(usb_gadget_register_driver);
@@ -2261,7 +2357,6 @@ int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
 {
 	struct fsl_ep *loop_ep;
 	unsigned long flags;
-	u32 portsc;
 
 	if (!udc_controller)
 		return -ENODEV;
@@ -2269,15 +2364,16 @@ int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
 	if (!driver || driver != udc_controller->driver || !driver->unbind)
 		return -EINVAL;
 
+	if(udc_controller->stopped)
+		dr_clk_gate(true);
+
 	if (udc_controller->transceiver)
 		(void)otg_set_peripheral(udc_controller->transceiver, 0);
 
-	/* open phy clock for following operation */
-	dr_phy_low_power_mode(udc_controller, false);
-
 	/* stop DR, disable intr */
 	dr_controller_stop(udc_controller);
 
+	udc_controller->pdata->port_enables = 0;
 	/* in fact, no needed */
 	udc_controller->usb_state = USB_STATE_ATTACHED;
 	udc_controller->ep0_dir = 0;
@@ -2299,14 +2395,11 @@ int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
 	udc_controller->gadget.dev.driver = 0;
 	udc_controller->driver = 0;
 
-	dr_wake_up_enable(udc_controller, false);
-
-	portsc = fsl_readl(&dr_regs->portsc1);
-	portsc |= PORTSCX_PHY_LOW_POWER_SPD;
-	fsl_writel(portsc, &dr_regs->portsc1);
+	if (udc_controller->gadget.is_otg) {
+		dr_wake_up_enable(udc_controller, true);
+	}
 
-	if (udc_controller->pdata->usb_clock_for_pm)
-		udc_controller->pdata->usb_clock_for_pm(false);
+	dr_phy_low_power_mode(udc_controller, true);
 
 	printk(KERN_INFO "unregistered gadget driver '%s'\r\n",
 	       driver->driver.name);
@@ -2705,14 +2798,6 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 	int ret = -ENODEV;
 	unsigned int i;
 	u32 dccparams;
-#ifndef CONFIG_USB_OTG
-	u32 portsc;
-#endif
-
-	if (strcmp(pdev->name, driver_name)) {
-		VDBG("Wrong device\n");
-		return -ENODEV;
-	}
 
 	udc_controller = kzalloc(sizeof(struct fsl_udc), GFP_KERNEL);
 	if (udc_controller == NULL) {
@@ -2729,6 +2814,7 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 		ret = -ENODEV;
 		goto err1a;
 	}
+	udc_controller->gadget.is_otg = 1;
 #endif
 
 	if ((pdev->dev.parent) &&
@@ -2768,6 +2854,9 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 		goto err2a;
 	}
 
+	/* Due to mx35/mx25's phy's bug */
+	reset_phy();
+
 	if (pdata->have_sysif_regs)
 		usb_sys_regs = (struct usb_sys_interface *)
 				((u32)dr_regs + USB_DR_SYS_OFFSET);
@@ -2826,12 +2915,6 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto err3;
 
-	if (udc_controller->transceiver) {
-		udc_controller->gadget.is_otg = 1;
-		/* now didn't support lpm in OTG mode*/
-		device_set_wakeup_capable(&pdev->dev, 0);
-	}
-
 	/* setup QH and epctrl for ep0 */
 	ep0_setup(udc_controller);
 
@@ -2874,20 +2957,29 @@ static int __init fsl_udc_probe(struct platform_device *pdev)
 #ifdef POSTPONE_FREE_LAST_DTD
 	last_free_td = NULL;
 #endif
+
 #ifndef CONFIG_USB_OTG
 	/* disable all INTR */
 	fsl_writel(0, &dr_regs->usbintr);
-
 	dr_wake_up_enable(udc_controller, false);
+#else
+	dr_wake_up_enable(udc_controller, true);
+#endif
+
+/*
+ * As mx25/mx35 does not implement clk_gate, should not let phy to low
+ * power mode due to IC bug
+ */
+#if !(defined CONFIG_ARCH_MX35 || defined CONFIG_ARCH_MX25)
+{
+	dr_phy_low_power_mode(udc_controller, true);
+}
+#endif
 	udc_controller->stopped = 1;
 
-	portsc = fsl_readl(&dr_regs->portsc1);
-	portsc |= PORTSCX_PHY_LOW_POWER_SPD;
-	fsl_writel(portsc, &dr_regs->portsc1);
+	/* let the gadget register function open the clk */
+	dr_clk_gate(false);
 
-	if (udc_controller->pdata->usb_clock_for_pm)
-		udc_controller->pdata->usb_clock_for_pm(false);
-#endif
 	create_proc_file();
 	return 0;
 
@@ -2914,9 +3006,6 @@ err1a:
  */
 static int __exit fsl_udc_remove(struct platform_device *pdev)
 {
-#ifndef CONFIG_USB_OTG
-	struct resource *res;
-#endif
 	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
 
 	DECLARE_COMPLETION(done);
@@ -2925,7 +3014,8 @@ static int __exit fsl_udc_remove(struct platform_device *pdev)
 		return -ENODEV;
 	udc_controller->done = &done;
 	/* open USB PHY clock */
-	dr_phy_low_power_mode(udc_controller, false);
+	if (udc_controller->stopped)
+		dr_clk_gate(true);
 
 	/* DR has been stopped in usb_gadget_unregister_driver() */
 	remove_proc_file();
@@ -2948,8 +3038,11 @@ static int __exit fsl_udc_remove(struct platform_device *pdev)
 	iounmap((u8 __iomem *)dr_regs);
 
 #ifndef CONFIG_USB_OTG
+{
+	struct resource *res;
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	release_mem_region(res->start, resource_size(res));
+}
 #endif
 
 	device_unregister(&udc_controller->gadget.dev);
@@ -2963,6 +3056,8 @@ static int __exit fsl_udc_remove(struct platform_device *pdev)
 	if (pdata->platform_uninit)
 		pdata->platform_uninit(pdata);
 
+	if (udc_controller->stopped)
+		dr_clk_gate(false);
 	return 0;
 }
 
@@ -2970,10 +3065,19 @@ static int udc_suspend(struct fsl_udc *udc)
 {
 	u32 mode, usbcmd;
 
-	/* open clock for register access */
-	if (udc_controller->pdata->usb_clock_for_pm)
-		udc_controller->pdata->usb_clock_for_pm(true);
-
+	/*
+	 * When it is the PM suspend routine and the device has no
+	 * abilities to wakeup system, it should not set wakeup enable.
+	 * Otherwise, the system will wakeup even the user only wants to
+	 * charge using usb
+	 */
+	if (udc_controller->gadget.dev.parent->power.status
+			== DPM_SUSPENDING) {
+		if (!device_may_wakeup(udc_controller->gadget.dev.parent))
+			dr_wake_up_enable(udc, false);
+		else
+			dr_wake_up_enable(udc, true);
+	}
 	mode = fsl_readl(&dr_regs->usbmode) & USB_MODE_CTRL_MODE_MASK;
 	usbcmd = fsl_readl(&dr_regs->usbcmd);
 
@@ -2984,9 +3088,8 @@ static int udc_suspend(struct fsl_udc *udc)
 	 * PM suspend.  Remember this fact, so that we will leave the
 	 * controller stopped at PM resume time.
 	 */
-	if (udc->stopped) {
+	if (udc->suspended) {
 		pr_debug("gadget already stopped, leaving early\n");
-		udc->already_stopped = 1;
 		goto out;
 	}
 
@@ -2995,22 +3098,30 @@ static int udc_suspend(struct fsl_udc *udc)
 		goto out;
 	}
 
+	/* For some buggy hardware designs, see comment of this function for detail */
+	udc_wait_b_session_low();
+
 	udc->stopped = 1;
-	/* if the suspend is not for switch to host in otg mode */
-	if ((!(udc->gadget.is_otg)) ||
-			(fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID)) {
-		dr_wake_up_enable(udc, true);
-		dr_phy_low_power_mode(udc, true);
-	}
 
 	/* stop the controller */
 	usbcmd = fsl_readl(&dr_regs->usbcmd) & ~USB_CMD_RUN_STOP;
 	fsl_writel(usbcmd, &dr_regs->usbcmd);
 
+	/* if the suspend is not for switch to host in otg mode */
+	if ((!(udc->gadget.is_otg)) ||
+			(fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID)) {
+		if (device_may_wakeup(udc_controller->gadget.dev.parent)) {
+			dr_wake_up_enable(udc, true);
+		}
+	}
+
+	dr_phy_low_power_mode(udc, true);
 	printk(KERN_INFO "USB Gadget suspended\n");
 out:
-	if (udc_controller->pdata->usb_clock_for_pm)
-		udc_controller->pdata->usb_clock_for_pm(false);
+	udc->suspended++;
+	if (udc->suspended > 2)
+		printk("ERROR: suspended times > 2\n");
+
 	return 0;
 }
 
@@ -3020,13 +3131,24 @@ out:
  -----------------------------------------------------------------*/
 static int fsl_udc_suspend(struct platform_device *pdev, pm_message_t state)
 {
+	int ret;
+#ifdef CONFIG_USB_OTG
+	if (udc_controller->transceiver->gadget == NULL)
+		return 0;
+#endif
+	if (udc_controller->stopped)
+		dr_clk_gate(true);
 	if (((!(udc_controller->gadget.is_otg)) ||
 			(fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID)) &&
 			(udc_controller->usb_state > USB_STATE_POWERED) &&
-			(udc_controller->usb_state < USB_STATE_SUSPENDED))
-		return -EBUSY;
+			(udc_controller->usb_state < USB_STATE_SUSPENDED)) {
+		return -EBUSY;/* keep the clk on */
+	}
+	else
+		ret = udc_suspend(udc_controller);
+	dr_clk_gate(false);
 
-	return udc_suspend(udc_controller);
+	return ret;
 }
 
 /*-----------------------------------------------------------------
@@ -3035,30 +3157,54 @@ static int fsl_udc_suspend(struct platform_device *pdev, pm_message_t state)
  *-----------------------------------------------------------------*/
 static int fsl_udc_resume(struct platform_device *pdev)
 {
-	pr_debug("%s(): stopped %d  already_stopped %d\n", __func__,
-		 udc_controller->stopped, udc_controller->already_stopped);
-
+	pr_debug("%s(): stopped %d  suspended %d\n", __func__,
+		 udc_controller->stopped, udc_controller->suspended);
+	printk("udc resume\n");
+#ifdef CONFIG_USB_OTG
+	if (udc_controller->transceiver->gadget == NULL)
+		return 0;
+#endif
+	if (udc_controller->stopped)
+		dr_clk_gate(true);
 	/*
 	 * If the controller was stopped at suspend time, then
 	 * don't resume it now.
 	 */
-	if (udc_controller->already_stopped) {
-		udc_controller->already_stopped = 0;
-		pr_debug("gadget was already stopped, leaving early\n");
-		return 0;
-	}
+		/*
+		 * If it is PM resume routine, the udc is at low power mode,
+		 * and the udc has no abilities to wakeup system, it should
+		 * set the abilities to wakeup itself. Otherwise, the usb
+		 * subsystem will not leave from low power mode.
+		 */
+		if (!device_may_wakeup(udc_controller->gadget.dev.parent) &&
+			udc_controller->gadget.dev.parent->power.status
+			== DPM_RESUMING){
+			dr_wake_up_enable(udc_controller, true);
+		}
 
+	if (--udc_controller->suspended) {
+		printk("gadget was already stopped, leaving early\n");
+		goto out;
+	}
 	/* Enable DR irq reg and set controller Run */
 	if (udc_controller->stopped) {
+		/* if in host mode, we need to do nothing */
+		if ((fsl_readl(&dr_regs->otgsc) & OTGSC_STS_USB_ID) == 0) {
+			goto out;
+		}
 		dr_wake_up_enable(udc_controller, false);
 		dr_phy_low_power_mode(udc_controller, false);
-		mdelay(1);
-
+		mdelay(10);
 		dr_controller_setup(udc_controller);
 		dr_controller_run(udc_controller);
 	}
 	udc_controller->usb_state = USB_STATE_ATTACHED;
 	udc_controller->ep0_dir = 0;
+out:
+	/* if udc is resume by otg id change and no device
+	 * connecting to the otg, otg will enter low power mode*/
+	if (udc_controller->stopped)
+		dr_clk_gate(false);
 
 	printk(KERN_INFO "USB Gadget resumed\n");
 	return 0;
diff --git a/drivers/usb/gadget/arcotg_udc.h b/drivers/usb/gadget/arcotg_udc.h
index 480d953dcf58..8d344acb8fef 100644
--- a/drivers/usb/gadget/arcotg_udc.h
+++ b/drivers/usb/gadget/arcotg_udc.h
@@ -266,6 +266,7 @@ struct usb_sys_interface {
 #define  PORTSCX_SPEED_BIT_POS                (26)
 
 /* OTGSC Register Bit Masks */
+#define  OTGSC_ID_CHANGE_IRQ_STS		(1 << 16)
 #define  OTGSC_B_SESSION_VALID_IRQ_EN           (1 << 27)
 #define  OTGSC_B_SESSION_VALID_IRQ_STS          (1 << 19)
 #define  OTGSC_B_SESSION_VALID                  (1 << 11)
@@ -365,6 +366,7 @@ struct usb_sys_interface {
 
 /* PHY control0 Register Bit Masks */
 #define	PHY_CTRL0_CONF2			(1 << 26)
+#define PHY_CTRL0_USBEN			(1 << 24) /* USB UTMI PHY Enable */
 
 /* USB UH2 CTRL Register Bits */
 #define USB_UH2_OVBWK_EN		(1 << 6) /* OTG VBUS Wakeup Enable */
@@ -592,9 +594,15 @@ struct fsl_udc {
 	struct otg_transceiver *transceiver;
 	unsigned softconnect:1;
 	unsigned vbus_active:1;
-	unsigned stopped:1;
 	unsigned remote_wakeup:1;
-	unsigned already_stopped:1;
+	/* we must distinguish the stopped and suspended state,
+	 * stopped means the udc enter lowpower mode, suspended
+	 * means the udc is suspended by system pm or by otg
+	 * switching to host mode.if the udc in suspended state
+	 * it also in the stopped state, while if the udc in
+	 * stopped state,it may not be in the suspended state*/
+	unsigned stopped:1;
+	int suspended;
 
 	struct ep_queue_head *ep_qh;	/* Endpoints Queue-Head */
 	struct fsl_req *status_req;	/* ep0 status request */
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index 59e85234fa0a..2e79b8c389a4 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -236,6 +236,7 @@ static int config_buf(struct usb_configuration *config,
 	int				len = USB_BUFSIZ - USB_DT_CONFIG_SIZE;
 	struct usb_function		*f;
 	int				status;
+	int				interfaceCount = 0;
 
 	/* write the config descriptor */
 	c = buf;
@@ -266,8 +267,16 @@ static int config_buf(struct usb_configuration *config,
 			descriptors = f->hs_descriptors;
 		else
 			descriptors = f->descriptors;
-		if (!descriptors)
+		if (f->hidden || !descriptors || descriptors[0] == NULL) {
+			for (; f != config->interface[interfaceCount];) {
+				interfaceCount++;
+				c->bNumInterfaces--;
+			}
 			continue;
+		}
+		for (; f != config->interface[interfaceCount];)
+			interfaceCount++;
+
 		status = usb_descriptor_fillbuf(next, len,
 			(const struct usb_descriptor_header **) descriptors);
 		if (status < 0)
@@ -756,11 +765,11 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 	case USB_REQ_GET_CONFIGURATION:
 		if (ctrl->bRequestType != USB_DIR_IN)
 			goto unknown;
-		if (cdev->config)
+		if (cdev->config) {
 			*(u8 *)req->buf = cdev->config->bConfigurationValue;
-		else
+			value = min(w_length, (u16) 1);
+		} else
 			*(u8 *)req->buf = 0;
-		value = min(w_length, (u16) 1);
 		break;
 
 	/* function drivers must handle get/set altsetting; if there's
@@ -810,6 +819,9 @@ unknown:
 		 */
 		if ((ctrl->bRequestType & USB_RECIP_MASK)
 				== USB_RECIP_INTERFACE) {
+			if (cdev->config == NULL)
+				return value;
+
 			f = cdev->config->interface[intf];
 			if (f && f->setup)
 				value = f->setup(f, ctrl);
@@ -824,6 +836,25 @@ unknown:
 				value = c->setup(c, ctrl);
 		}
 
+		/* If the vendor request is not processed (value < 0),
+		 * call all device registered configure setup callbacks
+		 * to process it.
+		 * This is used to handle the following cases:
+		 * - vendor request is for the device and arrives before
+		 * setconfiguration.
+		 * - Some devices are required to handle vendor request before
+		 * setconfiguration such as MTP, USBNET.
+		 */
+
+		if (value < 0) {
+			struct usb_configuration        *cfg;
+
+			list_for_each_entry(cfg, &cdev->configs, list) {
+			if (cfg && cfg->setup)
+				value = cfg->setup(cfg, ctrl);
+			}
+		}
+
 		goto done;
 	}
 
diff --git a/drivers/usb/gadget/f_acm.c b/drivers/usb/gadget/f_acm.c
index 7953948bfe4a..7dd1a8bbe382 100644
--- a/drivers/usb/gadget/f_acm.c
+++ b/drivers/usb/gadget/f_acm.c
@@ -761,3 +761,12 @@ int __init acm_bind_config(struct usb_configuration *c, u8 port_num)
 		kfree(acm);
 	return status;
 }
+
+int __init acm_function_add(struct usb_composite_dev *cdev,
+	struct usb_configuration *c)
+{
+	int ret = acm_bind_config(c, 0);
+	if (ret == 0)
+		gserial_setup(c->cdev->gadget, 1);
+	return ret;
+}
diff --git a/drivers/usb/gadget/file_storage.c b/drivers/usb/gadget/file_storage.c
index 66105ce49672..8b0a13202573 100644
--- a/drivers/usb/gadget/file_storage.c
+++ b/drivers/usb/gadget/file_storage.c
@@ -728,6 +728,7 @@ struct fsg_dev {
 #include "fsl_updater.h"
 #endif
 
+static int do_set_interface(struct fsg_dev *fsg, int altsetting);
 typedef void (*fsg_routine_t)(struct fsg_dev *);
 
 static int exception_in_progress(struct fsg_dev *fsg)
@@ -1108,6 +1109,14 @@ static void fsg_disconnect(struct usb_gadget *gadget)
 	struct fsg_dev		*fsg = get_gadget_data(gadget);
 
 	DBG(fsg, "disconnect or port reset\n");
+	/*
+	 * The disconnect exception will call do_set_config, and therefore will
+	 * visit controller registers. However it is a delayed event, and will be
+	 * handled at another process, so the controller maybe have already close the
+	 * usb clock.*/
+	if (fsg->new_config)
+		do_set_interface(fsg, -1);/* disable the interface */
+
 	raise_exception(fsg, FSG_STATE_DISCONNECT);
 }
 
diff --git a/drivers/usb/gadget/fsl_updater.c b/drivers/usb/gadget/fsl_updater.c
index 8b4b54f8cca7..50acce441a90 100644
--- a/drivers/usb/gadget/fsl_updater.c
+++ b/drivers/usb/gadget/fsl_updater.c
@@ -29,6 +29,7 @@ static int utp_init(struct fsg_dev *fsg)
 	INIT_LIST_HEAD(&utp_context.write);
 	mutex_init(&utp_context.lock);
 
+	/* the max message is 64KB */
 	utp_context.buffer = vmalloc(0x10000);
 	if (!utp_context.buffer)
 		return -EIO;
@@ -63,6 +64,7 @@ static void utp_user_data_free(struct utp_user_data *uud)
 	kfree(uud);
 }
 
+/* Get the number of element for list */
 static u32 count_list(struct list_head *l)
 {
 	u32 count = 0;
@@ -74,10 +76,11 @@ static u32 count_list(struct list_head *l)
 
 	return count;
 }
-
+/* The routine will not go on if utp_context.queue is empty */
 #define WAIT_ACTIVITY(queue) \
  wait_event_interruptible(utp_context.wq, !list_empty(&utp_context.queue))
 
+/* Called by userspace program (uuc) */
 static ssize_t utp_file_read(struct file *file,
 			     char __user *buf,
 			     size_t size,
@@ -109,12 +112,15 @@ static ssize_t utp_file_read(struct file *file,
 			"need to put %d\n", size, size_to_put);
 	}
 
+	/*
+	 * The user program has already finished data process,
+	 * go on getting data from the host
+	 */
 	wake_up(&utp_context.list_full_wq);
 
 	return size_to_put;
 }
 
-
 static ssize_t utp_file_write(struct file *file, const char __user *buf,
 				size_t size, loff_t *off)
 {
@@ -127,11 +133,13 @@ static ssize_t utp_file_write(struct file *file, const char __user *buf,
 		return -EACCES;
 	mutex_lock(&utp_context.lock);
 	list_add_tail(&uud->link, &utp_context.write);
+	/* Go on EXEC routine process */
 	wake_up(&utp_context.wq);
 	mutex_unlock(&utp_context.lock);
 	return size;
 }
 
+/* Will be called when the host wants to get the sense data */
 static int utp_get_sense(struct fsg_dev *fsg)
 {
 	if (UTP_CTX(fsg)->processed == 0)
@@ -186,6 +194,7 @@ static int utp_do_read(struct fsg_dev *fsg, void *data, size_t size)
 		/* Perform the read */
 		pr_info("Copied to %p, %d bytes started from %d\n",
 				bh->buf, amount, size - amount_left);
+		/* from upt buffer to file_storeage buffer */
 		memcpy(bh->buf, data + size - amount_left, amount);
 		amount_left  -= amount;
 		fsg->residue -= amount;
@@ -196,6 +205,7 @@ static int utp_do_read(struct fsg_dev *fsg, void *data, size_t size)
 		/* Send this buffer and go read some more */
 		bh->inreq->zero = 0;
 
+		/* USB Physical transfer: Data from device to host */
 		start_transfer(fsg, fsg->bulk_in, bh->inreq,
 				&bh->inreq_busy, &bh->state);
 
@@ -326,8 +336,8 @@ static void utp_poll(struct fsg_dev *fsg)
 
 	if (uud) {
 		if (uud->data.flags & UTP_FLAG_STATUS) {
-			pr_debug("%s: exit with status %d\n", __func__,
-				uud->data.status);
+			printk(KERN_WARNING "%s: exit with status %d\n",
+					__func__, uud->data.status);
 			UTP_SS_EXIT(fsg, uud->data.status);
 		} else {
 			pr_debug("%s: pass\n", __func__);
@@ -356,11 +366,16 @@ static int utp_exec(struct fsg_dev *fsg,
 	mutex_lock(&ctx->lock);
 	list_add_tail(&uud2r->link, &ctx->read);
 	mutex_unlock(&ctx->lock);
+	/* wake up the read routine */
 	wake_up(&ctx->wq);
 
 	if (command[0] == '!')	/* there will be no response */
 		return 0;
 
+	/*
+	 * the user program (uuc) will return utp_message
+	 * and add list to write list
+	 */
 	WAIT_ACTIVITY(write);
 
 	mutex_lock(&ctx->lock);
@@ -382,21 +397,19 @@ static int utp_exec(struct fsg_dev *fsg,
 	if (uud->data.flags & UTP_FLAG_DATA) {
 		memcpy(ctx->buffer, uud->data.data, uud->data.bufsize);
 		UTP_SS_SIZE(fsg, uud->data.bufsize);
-		utp_user_data_free(uud);
-		return 0;
-	}
-
-	if (uud->data.flags & UTP_FLAG_REPORT_BUSY) {
-		utp_user_data_free(uud);
+	} else if (uud->data.flags & UTP_FLAG_REPORT_BUSY) {
 		ctx->counter = 0xFFFF;
 		UTP_SS_BUSY(fsg, ctx->counter);
-		return 0;
+	} else if (uud->data.flags & UTP_FLAG_STATUS) {
+		printk(KERN_WARNING "%s: exit with status %d\n", __func__,
+				uud->data.status);
+		UTP_SS_EXIT(fsg, uud->data.status);
+	} else {
+		pr_debug("%s: pass\n", __func__);
+		UTP_SS_PASS(fsg);
 	}
-
 	utp_user_data_free(uud);
-	UTP_SS_PASS(fsg);
-
-	return -1;
+	return 0;
 }
 
 static int utp_send_status(struct fsg_dev *fsg)
@@ -470,16 +483,17 @@ static int utp_handle_message(struct fsg_dev *fsg,
 	case UTP_EXEC:
 		pr_debug("%s: EXEC\n", __func__);
 		data = kzalloc(fsg->data_size, GFP_KERNEL);
+		/* copy data from usb buffer to utp buffer */
 		utp_do_write(fsg, data, fsg->data_size);
 		utp_exec(fsg, data, fsg->data_size, param);
 		kfree(data);
 		break;
-	case UTP_GET:
+	case UTP_GET: /* data from device to host */
 		pr_debug("%s: GET, %d bytes\n", __func__, fsg->data_size);
 		r = utp_do_read(fsg, UTP_CTX(fsg)->buffer, fsg->data_size);
 		UTP_SS_PASS(fsg);
 		break;
-	case UTP_PUT:
+	case UTP_PUT: /* data from host to device */
 		pr_debug("%s: PUT, %d bytes\n", __func__, fsg->data_size);
 		uud2r = utp_user_data_alloc(fsg->data_size);
 		uud2r->data.bufsize = fsg->data_size;
@@ -490,6 +504,37 @@ static int utp_handle_message(struct fsg_dev *fsg,
 		list_add_tail(&uud2r->link, &UTP_CTX(fsg)->read);
 		mutex_unlock(&UTP_CTX(fsg)->lock);
 		wake_up(&UTP_CTX(fsg)->wq);
+		/*
+		 * Return PASS or FAIL according to uuc's status
+		 * Please open it if need to check uuc's status
+		 * and use another version uuc
+		 */
+#if 0
+		struct utp_user_data *uud = NULL;
+		struct utp_context *ctx;
+		WAIT_ACTIVITY(write);
+		ctx = UTP_CTX(fsg);
+		mutex_lock(&ctx->lock);
+
+		if (!list_empty(&ctx->write))
+			uud = list_first_entry(&ctx->write,
+					struct utp_user_data, link);
+
+		mutex_unlock(&ctx->lock);
+		if (uud) {
+			if (uud->data.flags & UTP_FLAG_STATUS) {
+				printk(KERN_WARNING "%s: exit with status %d\n",
+					 __func__, uud->data.status);
+				UTP_SS_EXIT(fsg, uud->data.status);
+			} else {
+				pr_debug("%s: pass\n", __func__);
+				UTP_SS_PASS(fsg);
+			}
+			utp_user_data_free(uud);
+		} else{
+			UTP_SS_PASS(fsg);
+		}
+#endif
 		UTP_SS_PASS(fsg);
 
 		wait_event_interruptible(UTP_CTX(fsg)->list_full_wq,
diff --git a/drivers/usb/gadget/fsl_updater.h b/drivers/usb/gadget/fsl_updater.h
index 44329a9af58a..70e4defa1a9c 100644
--- a/drivers/usb/gadget/fsl_updater.h
+++ b/drivers/usb/gadget/fsl_updater.h
@@ -59,6 +59,7 @@ static int utp_handle_message(struct fsg_dev *fsg,
 #define UTP_SS_BUSY(fsg, r)	utp_set_sense(fsg, UTP_REPLY_BUSY, (u64)r)
 #define UTP_SS_SIZE(fsg, r)	utp_set_sense(fsg, UTP_REPLY_SIZE, (u64)r)
 
+/* the structure of utp message which is mapped to 16-byte SCSI CBW's CDB */
 #pragma pack(1)
 struct utp_msg {
 	u8  f0;
diff --git a/drivers/usb/host/ehci-arc.c b/drivers/usb/host/ehci-arc.c
index 21133fb8e47a..5cfcf169e7c7 100644
--- a/drivers/usb/host/ehci-arc.c
+++ b/drivers/usb/host/ehci-arc.c
@@ -23,9 +23,28 @@
 #include <linux/fsl_devices.h>
 #include <linux/usb/otg.h>
 
+#include "../core/usb.h"
 #include "ehci-fsl.h"
 #include <mach/fsl_usb.h>
 
+extern int usb_host_wakeup_irq(struct device *wkup_dev);
+extern void usb_host_set_wakeup(struct device *wkup_dev, bool para);
+static void fsl_usb_lowpower_mode(struct fsl_usb2_platform_data *pdata, bool enable)
+{
+	if (enable){
+		if (pdata->phy_lowpower_suspend)
+			pdata->phy_lowpower_suspend(true);
+	} else {
+		if (pdata->phy_lowpower_suspend)
+			pdata->phy_lowpower_suspend(false);
+	}
+}
+
+static void fsl_usb_clk_gate(struct fsl_usb2_platform_data *pdata, bool enable)
+{
+	if (pdata->usb_clock_for_pm)
+		pdata->usb_clock_for_pm(enable);
+}
 #undef EHCI_PROC_PTC
 #ifdef EHCI_PROC_PTC		/* /proc PORTSC:PTC support */
 /*
@@ -90,8 +109,39 @@ static int ehci_testmode_init(struct ehci_hcd *ehci)
 #endif	/* /proc PORTSC:PTC support */
 
 
-/* configure so an HC device and id are always provided */
-/* always called with process context; sleeping is OK */
+/*
+ * This irq is used to open the hw access and let usb_hcd_irq process the usb event
+ * ehci_fsl_pre_irq will be called before usb_hcd_irq
+ */
+static irqreturn_t ehci_fsl_pre_irq(int irq, void *dev)
+{
+	struct platform_device *pdev = (struct platform_device *)dev;
+	struct usb_hcd *hcd = platform_get_drvdata(pdev);
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	struct fsl_usb2_platform_data *pdata;
+
+	pdata = hcd->self.controller->platform_data;
+
+	/* if it is an otg module and in b device mode, we need to do noting here */
+	if (ehci->transceiver && !ehci->transceiver->default_a)
+		return IRQ_NONE;
+
+	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+		/* Need to open clk for accessing the register */
+		fsl_usb_clk_gate(hcd->self.controller->platform_data, true);
+		/* if receive a remote wakeup interrrupt after suspend */
+		if (usb_host_wakeup_irq(hcd->self.controller)) {
+			printk("host wakeup event happens\n");
+			/* disable remote wake up irq */
+			usb_host_set_wakeup(hcd->self.controller, false);
+			fsl_usb_lowpower_mode(pdata, false);
+			set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+		}else {
+			fsl_usb_clk_gate(hcd->self.controller->platform_data, false);
+		}
+	}
+	return IRQ_NONE;
+}
 
 /**
  * usb_hcd_fsl_probe - initialize FSL-based HCDs
@@ -182,10 +232,19 @@ int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	fsl_platform_set_host_mode(hcd);
 	hcd->power_budget = pdata->power_budget;
 
-	retval = usb_add_hcd(hcd, irq, IRQF_DISABLED | IRQF_SHARED);
+	/*
+	 * The ehci_fsl_pre_irq must be registered before usb_hcd_irq, in that case
+	 * it can be called before usb_hcd_irq when irq occurs
+	 */
+	retval = request_irq(irq, ehci_fsl_pre_irq, IRQF_SHARED,
+			"fsl ehci pre interrupt", (void *)pdev);
 	if (retval != 0)
 		goto err4;
 
+	retval = usb_add_hcd(hcd, irq, IRQF_DISABLED | IRQF_SHARED);
+	if (retval != 0)
+		goto err5;
+
 	fsl_platform_set_vbus_power(pdata, 1);
 
 	if (pdata->operating_mode == FSL_USB2_DR_OTG) {
@@ -199,7 +258,7 @@ int usb_hcd_fsl_probe(const struct hc_driver *driver,
 		if (!ehci->transceiver) {
 			printk(KERN_ERR "can't find transceiver\n");
 			retval = -ENODEV;
-			goto err4;
+			goto err5;
 		}
 
 		retval = otg_set_host(ehci->transceiver, &ehci_to_hcd(ehci)->self);
@@ -216,7 +275,8 @@ int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	fsl_platform_set_ahb_burst(hcd);
 	ehci_testmode_init(hcd_to_ehci(hcd));
 	return retval;
-
+err5:
+	free_irq(irq, (void *)pdev);
 err4:
 	iounmap(hcd->regs);
 err3:
@@ -231,9 +291,6 @@ err1:
 	return retval;
 }
 
-/* may be called without controller electrically present */
-/* may be called with controller, bus, and devices active */
-
 /**
  * usb_hcd_fsl_remove - shutdown processing for FSL-based HCDs
  * @dev: USB Host Controller being removed
@@ -324,6 +381,59 @@ static int ehci_fsl_reinit(struct ehci_hcd *ehci)
 	return 0;
 }
 
+static int ehci_fsl_bus_suspend(struct usb_hcd *hcd)
+{
+	int ret = 0;
+	struct fsl_usb2_platform_data *pdata;
+	pdata = hcd->self.controller->platform_data;
+	pr_debug("%s, %s\n", __func__, pdata->name);
+
+	/* the host is already at low power mode */
+	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+		return 0;
+	}
+
+	pr_debug("%s, it is the host mode, %s\n", __func__, pdata->name);
+
+	ehci_bus_suspend(hcd);
+
+	if (pdata->platform_suspend)
+		pdata->platform_suspend(pdata);
+
+	usb_host_set_wakeup(hcd->self.controller, true);
+	fsl_usb_lowpower_mode(pdata, true);
+	fsl_usb_clk_gate(hcd->self.controller->platform_data, false);
+	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+
+	return ret;
+}
+
+static int ehci_fsl_bus_resume(struct usb_hcd *hcd)
+{
+	int ret = 0;
+	struct fsl_usb2_platform_data *pdata;
+
+	pdata = hcd->self.controller->platform_data;
+	pr_debug("%s, %s\n", __func__, pdata->name);
+
+	/* if it is a remote wakeup, it will open clock and clear PHCD automatically */
+	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+		set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+		fsl_usb_clk_gate(hcd->self.controller->platform_data, true);
+		usb_host_set_wakeup(hcd->self.controller, false);
+		fsl_usb_lowpower_mode(pdata, false);
+	}
+
+	if (pdata->platform_resume)
+		pdata->platform_resume(pdata);
+	ret = ehci_bus_resume(hcd);
+	if (ret)
+		return ret;
+
+	return ret;
+}
+
+
 /* called during probe() after chip reset completes */
 static int ehci_fsl_setup(struct usb_hcd *hcd)
 {
@@ -396,8 +506,8 @@ static const struct hc_driver ehci_fsl_hc_driver = {
 	 */
 	.hub_status_data = ehci_hub_status_data,
 	.hub_control = ehci_hub_control,
-	.bus_suspend = ehci_bus_suspend,
-	.bus_resume = ehci_bus_resume,
+	.bus_suspend = ehci_fsl_bus_suspend,
+	.bus_resume = ehci_fsl_bus_resume,
 	.start_port_reset = ehci_start_port_reset,
 	.relinquish_port = ehci_relinquish_port,
 	.port_handed_over = ehci_port_handed_over,
@@ -438,13 +548,36 @@ static int ehci_fsl_drv_suspend(struct platform_device *pdev,
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
-	u32 tmp, port_status;
+	struct usb_device *roothub = hcd->self.root_hub;
+	u32 port_status;
 	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
 
-	if (device_may_wakeup(&(pdev->dev))) {
-		/* Need open clock for register access */
-		if (pdata->usb_clock_for_pm)
-			pdata->usb_clock_for_pm(true);
+	/* Only handles OTG mode switch event, system suspend event will be done in bus suspend */
+	if (pdev->dev.power.status == DPM_SUSPENDING){
+		pr_debug("%s, system pm event \n", __func__);
+		if (!device_may_wakeup(&(pdev->dev))){
+			/* Need open clock for register access */
+			if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags))
+				fsl_usb_clk_gate(hcd->self.controller->platform_data, true);
+			usb_host_set_wakeup(hcd->self.controller, false);
+			fsl_usb_clk_gate(hcd->self.controller->platform_data, false);
+		}
+		return 0;
+	}
+	/* only the otg host can go here */
+	/* wait for all usb device on the hcd dettached */
+	while(roothub->children[0] != NULL)
+		msleep(1);
+	if ((pdata->operating_mode != FSL_USB2_MPH_HOST) && (!(hcd->state & HC_STATE_SUSPENDED)))
+	{
+		usb_lock_device(roothub);
+		usb_external_suspend_device(roothub, PMSG_USER_SUSPEND);
+		usb_unlock_device(roothub);
+	}
+
+	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+		fsl_usb_clk_gate(hcd->self.controller->platform_data, true);
+		set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
 	}
 
 #ifdef DEBUG
@@ -457,27 +590,9 @@ static int ehci_fsl_drv_suspend(struct platform_device *pdev,
 	       pdata->suspended, pdata->already_suspended, mode, tmp);
 #endif
 
-	/*
-	 * If the controller is already suspended, then this must be a
-	 * PM suspend.  Remember this fact, so that we will leave the
-	 * controller suspended at PM resume time.
-	 */
-	if (pdata->suspended) {
-		pr_debug("%s: already suspended, leaving early\n", __func__);
-		pdata->already_suspended = 1;
-		goto err1;
-	}
-
-	pr_debug("%s: suspending...\n", __func__);
-
 	printk(KERN_INFO "USB Host suspended\n");
 
 	port_status = ehci_readl(ehci, &ehci->regs->port_status[0]);
-	pdev->dev.power.power_state = PMSG_SUSPEND;
-
-	/* ignore non-host interrupts */
-	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-
 	/* save EHCI registers */
 	pdata->pm_command = ehci_readl(ehci, &ehci->regs->command);
 	pdata->pm_command &= ~CMD_RUN;
@@ -496,25 +611,11 @@ static int ehci_fsl_drv_suspend(struct platform_device *pdev,
 
 	/* clear PHCD bit */
 	pdata->pm_portsc &= ~PORT_PHCD;
-
-	pdata->suspended = 1;
-
-	if (!device_may_wakeup(&(pdev->dev))) {
-		/* clear PP to cut power to the port */
-		tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
-		tmp &= ~PORT_POWER;
-		ehci_writel(ehci, tmp, &ehci->regs->port_status[0]);
-		goto err1;
-	}
-
-	tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
-
-	if (pdata->platform_suspend)
-		pdata->platform_suspend(pdata);
-err1:
-	if (device_may_wakeup(&(pdev->dev))) {
-		if (pdata->usb_clock_for_pm)
-			pdata->usb_clock_for_pm(false);
+	if (test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+		//fsl_usb_lowpower_mode(pdata ,true);
+		//usb_host_set_wakeup(hcd->self.controller, true);
+		fsl_usb_clk_gate(hcd->self.controller->platform_data, false);
+		clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
 	}
 	return 0;
 }
@@ -523,47 +624,35 @@ static int ehci_fsl_drv_resume(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
 	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	struct usb_device *roothub = hcd->self.root_hub;
 	u32 tmp;
 	struct fsl_usb2_platform_data *pdata = pdev->dev.platform_data;
-
-	pr_debug("%s('%s'): suspend=%d already_suspended=%d\n", __func__,
-		pdata->name, pdata->suspended, pdata->already_suspended);
-
-	/*
-	 * If the controller was already suspended at suspend time,
-	 * then don't resume it now.
-	 */
-	if (pdata->already_suspended) {
-		pr_debug("already suspended, leaving early\n");
-		pdata->already_suspended = 0;
-		return 0;
-	}
-
-	if (!pdata->suspended) {
-		pr_debug("not suspended, leaving early\n");
+	/* Only handles OTG mode switch event */
+	if (pdev->dev.power.status == DPM_RESUMING){
+		pr_debug("%s, system pm event \n", __func__);
+		if (hcd->self.is_b_host) {
+			if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+				fsl_usb_clk_gate(hcd->self.controller->platform_data, true);
+			}
+			usb_host_set_wakeup(hcd->self.controller, true);
+
+			if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+				fsl_usb_clk_gate(hcd->self.controller->platform_data, false);
+			}
+		}
 		return 0;
 	}
-
-	/* If hcd is resumed by non-usb wakeup events,
-	 * then usb clocks are still not open when come here */
-	if (device_may_wakeup(&(pdev->dev))) {
-		/* Need open clock for register access */
-		if (pdata->usb_clock_for_pm)
-			pdata->usb_clock_for_pm(true);
+	if (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags)) {
+		set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
+		fsl_usb_clk_gate(hcd->self.controller->platform_data, true);
+		//usb_host_set_wakeup(hcd->self.controller, false);
+		//fsl_usb_lowpower_mode(pdata, false);
 	}
 
-	tmp = ehci_readl(ehci, &ehci->regs->port_status[0]);
-
-	pdata->suspended = 0;
-
-	pr_debug("%s resuming...\n", __func__);
-
+	printk("USB Host resume ... %s\n", pdata->name);
 	/* set host mode */
 	fsl_platform_set_host_mode(hcd);
 
-	if (pdata->platform_resume)
-		pdata->platform_resume(pdata);
-
 	/* restore EHCI registers */
 	ehci_writel(ehci, pdata->pm_portsc, &ehci->regs->port_status[0]);
 	ehci_writel(ehci, pdata->pm_command, &ehci->regs->command);
@@ -575,32 +664,21 @@ static int ehci_fsl_drv_resume(struct platform_device *pdev)
 	ehci_writel(ehci, pdata->pm_configured_flag,
 		    &ehci->regs->configured_flag);
 
-	/* set bit should be done by wakeup irq routine if may wakeup */
-	if (!device_may_wakeup(&(pdev->dev)))
-		set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
-	else
-		while (!test_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags))
-			msleep(1);
-
-	pdev->dev.power.power_state = PMSG_ON;
 
 	tmp = ehci_readl(ehci, &ehci->regs->command);
 	tmp |= CMD_RUN;
 	ehci_writel(ehci, tmp, &ehci->regs->command);
 
-	usb_hcd_resume_root_hub(hcd);
-
-	printk(KERN_INFO "USB Host resumed\n");
-
-	if (device_may_wakeup(&(pdev->dev))) {
-		if (pdata->usb_clock_for_pm)
-			pdata->usb_clock_for_pm(false);
+	if ((hcd->state & HC_STATE_SUSPENDED)){
+		usb_lock_device(roothub);
+		usb_external_resume_device(roothub, PMSG_USER_RESUME);
+		usb_unlock_device(roothub);
 	}
 
+	printk(KERN_INFO "USB Host resume ok\n");
 	return 0;
 }
-#endif				/* CONFIG_USB_OTG */
-
+#endif
 MODULE_ALIAS("platform:fsl-ehci");
 
 static struct platform_driver ehci_fsl_driver = {
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index de459bbd1eb1..44ff32306362 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -112,6 +112,7 @@ static int ehci_bus_suspend (struct usb_hcd *hcd)
 	int			port;
 	int			mask;
 
+	printk("%s\n", __func__);
 	ehci_dbg(ehci, "suspend root hub\n");
 
 	if (time_before (jiffies, ehci->next_statechange))
diff --git a/drivers/usb/otg/fsl_otg.c b/drivers/usb/otg/fsl_otg.c
index b1454886fd7a..81a2985632b6 100644
--- a/drivers/usb/otg/fsl_otg.c
+++ b/drivers/usb/otg/fsl_otg.c
@@ -34,6 +34,7 @@
 #include <linux/init.h>
 #include <linux/reboot.h>
 #include <linux/timer.h>
+#include <linux/jiffies.h>
 #include <linux/list.h>
 #include <linux/usb.h>
 #include <linux/device.h>
@@ -41,6 +42,7 @@
 #include <linux/usb/gadget.h>
 #include <linux/workqueue.h>
 #include <linux/time.h>
+#include <linux/usb/fsl_xcvr.h>
 #include <linux/fsl_devices.h>
 #include <linux/platform_device.h>
 #include <linux/irq.h>
@@ -60,6 +62,8 @@
 #define DRIVER_DESC "Freescale USB OTG Driver"
 #define DRIVER_INFO DRIVER_VERSION " " DRIVER_DESC
 
+#define TIMER_FREQ 1000 /* 100 ms*/
+#define IDLE_TIME  5000 /* 1000 ms */
 
 MODULE_DESCRIPTION("Freescale USB OTG Transceiver Driver");
 
@@ -89,6 +93,13 @@ static struct fsl_otg_config fsl_otg_initdata = {
 	.otg_port = 1,
 };
 
+/* the timer is used to monitor the otg loading, if idle for some times
+ * we will close the otg clk
+ */
+static unsigned long last_busy;
+static bool clk_stopped;
+static struct timer_list monitor_timer;
+
 int write_ulpi(u8 addr, u8 data)
 {
 	u32 temp;
@@ -136,17 +147,10 @@ void fsl_otg_dischrg_vbus(int on)
 }
 
 /* A-device driver vbus, controlled through PP bit in PORTSC */
-void fsl_otg_drv_vbus(int on)
+void fsl_otg_drv_vbus(struct fsl_usb2_platform_data *pdata, int on)
 {
-/*	if (on)
-		usb_dr_regs->portsc =
-		    cpu_to_le32((le32_to_cpu(usb_dr_regs->portsc) &
-				 ~PORTSC_W1C_BITS) | PORTSC_PORT_POWER);
-	else
-		usb_dr_regs->portsc =
-		    cpu_to_le32(le32_to_cpu(usb_dr_regs->portsc) &
-				~PORTSC_W1C_BITS & ~PORTSC_PORT_POWER);
-*/
+	if (pdata->xcvr_ops && pdata->xcvr_ops->set_vbus_power)
+		pdata->xcvr_ops->set_vbus_power(pdata->xcvr_ops, pdata, on);
 }
 
 /*
@@ -395,6 +399,61 @@ int fsl_otg_tick_timer(void)
 	return expired;
 }
 
+static void fsl_otg_clk_gate(bool on)
+{
+	struct device *dev = fsl_otg_dev->otg.dev;
+	struct fsl_usb2_platform_data *pdata;
+
+	if (dev) {
+		pdata = dev->platform_data;
+		if (pdata && pdata->usb_clock_for_pm)
+		    pdata->usb_clock_for_pm(on);
+	}
+}
+
+static void fsl_otg_clk_ctl(void)
+{
+	if (clk_stopped){
+		fsl_otg_clk_gate(true);
+		clk_stopped = false;
+	}
+	last_busy = jiffies;
+}
+
+static void fsl_otg_loading_monitor(unsigned long data)
+{
+	unsigned long now = jiffies;
+	if (!clk_stopped){
+		if (time_after(now, last_busy + msecs_to_jiffies(IDLE_TIME))){
+			printk("otg is idle for some times,so we close the clock %x\n", le32_to_cpu(usb_dr_regs->otgsc));
+			clk_stopped = true;
+			fsl_otg_clk_gate(false);
+			printk("close otg clk ok\n");
+		}
+	}
+	mod_timer(&monitor_timer, jiffies + msecs_to_jiffies(TIMER_FREQ));
+}
+
+/**
+ * Enable vbus interrupt
+ * The otg cares USB_ID interrupt
+ * The device cares B Sesstion Valid
+ */
+static void b_session_irq_enable(bool enable)
+{
+	int osc = le32_to_cpu(usb_dr_regs->otgsc);
+	/* The other interrupts' status should not be cleared */
+	osc &= ~(OTGSC_INTSTS_USB_ID | OTGSC_INTSTS_A_VBUS_VALID
+		| OTGSC_INTSTS_A_SESSION_VALID | OTGSC_INTSTS_B_SESSION_VALID);
+	osc |= OTGSC_INTSTS_B_SESSION_VALID;
+
+	if (enable)
+		osc |= OTGSC_INTR_B_SESSION_VALID_EN;
+	else
+		osc &= ~OTGSC_INTR_B_SESSION_VALID_EN;
+	usb_dr_regs->otgsc = cpu_to_le32(osc);
+}
+
 /* Reset controller, not reset the bus */
 void otg_reset_controller(void)
 {
@@ -438,7 +497,7 @@ int fsl_otg_start_host(struct otg_fsm *fsm, int on)
 				retval = host_pdrv->resume(host_pdev);
 				if (fsm->id) {
 					/* default-b */
-					fsl_otg_drv_vbus(1);
+					fsl_otg_drv_vbus(dev->platform_data, 1);
 					/* Workaround: b_host can't driver
 					 * vbus, but PP in PORTSC needs to
 					 * be 1 for host to work.
@@ -463,7 +522,7 @@ int fsl_otg_start_host(struct otg_fsm *fsm, int on)
 							otg_suspend_state);
 				if (fsm->id)
 					/* default-b */
-					fsl_otg_drv_vbus(0);
+					fsl_otg_drv_vbus(dev->platform_data, 0);
 			}
 			otg_dev->host_working = 0;
 		}
@@ -481,7 +540,6 @@ int fsl_otg_start_gadget(struct otg_fsm *fsm, int on)
 	struct device *dev;
 	struct platform_driver *gadget_pdrv;
 	struct platform_device *gadget_pdev;
-
 	if (!xceiv->gadget || !xceiv->gadget->dev.parent)
 		return -ENODEV;
 
@@ -517,7 +575,6 @@ static int fsl_otg_set_host(struct otg_transceiver *otg_p, struct usb_bus *host)
 
 	if (host) {
 		VDBG("host off......\n");
-
 		otg_p->host->otg_port = fsl_otg_initdata.otg_port;
 		otg_p->host->is_b_host = otg_dev->fsm.id;
 		/* must leave time for khubd to finish its thing
@@ -634,11 +691,29 @@ static void fsl_otg_event(struct work_struct *work)
 {
 	struct fsl_otg *og = container_of(work, struct fsl_otg, otg_event.work);
 	struct otg_fsm *fsm = &og->fsm;
+	struct otg_transceiver *otg = &og->otg;
+
+	otg->default_a = (fsm->id == 0);
+	/* clear conn information */
+	if (fsm->id)
+		fsm->b_conn = 0;
+	else
+		fsm->a_conn = 0;
+
+	if (otg->host)
+		otg->host->is_b_host = fsm->id;
+	if (otg->gadget)
+		otg->gadget->is_a_peripheral = !fsm->id;
 
 	if (fsm->id) {		/* switch to gadget */
+		b_session_irq_enable(true);
 		fsl_otg_start_host(fsm, 0);
 		otg_drv_vbus(fsm, 0);
 		fsl_otg_start_gadget(fsm, 1);
+	}else {			/* switch to host */
+		fsl_otg_start_gadget(fsm, 0);
+		otg_drv_vbus(fsm, 1);
+		fsl_otg_start_host(fsm, 1);
 	}
 }
 
@@ -678,12 +753,13 @@ irqreturn_t fsl_otg_isr_gpio(int irq, void *dev_id)
 	struct otg_fsm *fsm;
 	struct fsl_usb2_platform_data *pdata =
 		(struct fsl_usb2_platform_data *)dev_id;
-	struct fsl_otg *p_otg;
+	struct fsl_otg *f_otg;
 	struct otg_transceiver *otg_trans = otg_get_transceiver();
-	p_otg = container_of(otg_trans, struct fsl_otg, otg);
-	fsm = &p_otg->fsm;
 	int value;
+	f_otg = container_of(otg_trans, struct fsl_otg, otg);
+	fsm = &f_otg->fsm;
 
+	fsl_otg_clk_ctl();
 	if (pdata->id_gpio == 0)
 		return IRQ_NONE;
 
@@ -695,35 +771,25 @@ irqreturn_t fsl_otg_isr_gpio(int irq, void *dev_id)
 		set_irq_type(gpio_to_irq(pdata->id_gpio), IRQ_TYPE_LEVEL_HIGH);
 
 
-	if (value == p_otg->fsm.id)
+	if (value == f_otg->fsm.id)
 		return IRQ_HANDLED;
 
-	p_otg->fsm.id = value;
-
-	otg_trans->default_a = (fsm->id == 0);
-	/* clear conn information */
-	if (fsm->id)
-		fsm->b_conn = 0;
-	else
-		fsm->a_conn = 0;
-
-	if (otg_trans->host)
-		otg_trans->host->is_b_host = fsm->id;
-	if (otg_trans->gadget)
-		otg_trans->gadget->is_a_peripheral = !fsm->id;
-
-	VDBG("ID int (ID is %d)\n", fsm->id);
-	if (fsm->id) {  /* switch to gadget */
-		schedule_delayed_work(&p_otg->otg_event, 100);
+	f_otg->fsm.id = value;
 
-	} else {        /* switch to host */
-		cancel_delayed_work(&p_otg->otg_event);
-		fsl_otg_start_gadget(fsm, 0);
-		otg_drv_vbus(fsm, 1);
-		fsl_otg_start_host(fsm, 1);
+	cancel_delayed_work(&f_otg->otg_event);
+	schedule_delayed_work(&f_otg->otg_event, msecs_to_jiffies(10));
+	/* if host mode, we should clear B_SESSION_VLD event and disable
+	 * B_SESSION_VLD irq
+	 */
+	if (!f_otg->fsm.id) {
+		b_session_irq_enable(false);
+	}else {
+		//b_session_irq_enable(true);
 	}
+
 	return IRQ_HANDLED;
 }
+
 /* Interrupt handler.  OTG/host/peripheral share the same int line.
  * OTG driver clears OTGSC interrupts and leaves USB interrupts
  * intact.  It needs to have knowledge of some USB interrupts
@@ -731,60 +797,70 @@ irqreturn_t fsl_otg_isr_gpio(int irq, void *dev_id)
  */
 irqreturn_t fsl_otg_isr(int irq, void *dev_id)
 {
-	struct otg_fsm *fsm = &((struct fsl_otg *)dev_id)->fsm;
-	struct otg_transceiver *otg = &((struct fsl_otg *)dev_id)->otg;
+	struct fsl_otg *fotg = (struct fsl_otg *)dev_id;
+	struct otg_transceiver *otg = &fotg->otg;
 	u32 otg_int_src, otg_sc;
+	irqreturn_t ret = IRQ_NONE;
 
+	fsl_otg_clk_ctl();
 	otg_sc = le32_to_cpu(usb_dr_regs->otgsc);
 	otg_int_src = otg_sc & OTGSC_INTSTS_MASK & (otg_sc >> 8);
 
-	/* Only clear otg interrupts */
-	usb_dr_regs->otgsc |= cpu_to_le32(otg_sc & OTGSC_INTSTS_MASK);
+	/* Only clear otg interrupts, expect B_SESSION_VALID,
+	 * Leave it to be handled by arcotg_udc */
+	usb_dr_regs->otgsc = ((usb_dr_regs->otgsc | cpu_to_le32(otg_sc & OTGSC_INTSTS_MASK))&
+				(~OTGSC_INTSTS_B_SESSION_VALID));
 
 	/*FIXME: ID change not generate when init to 0 */
-	fsm->id = (otg_sc & OTGSC_STS_USB_ID) ? 1 : 0;
-	otg->default_a = (fsm->id == 0);
+	fotg->fsm.id = (otg_sc & OTGSC_STS_USB_ID) ? 1 : 0;
+	otg->default_a = (fotg->fsm.id == 0);
 
 	/* process OTG interrupts */
 	if (otg_int_src) {
 		if (otg_int_src & OTGSC_INTSTS_USB_ID) {
-			fsm->id = (otg_sc & OTGSC_STS_USB_ID) ? 1 : 0;
-			otg->default_a = (fsm->id == 0);
-			/* clear conn information */
-			if (fsm->id)
-				fsm->b_conn = 0;
-			else
-				fsm->a_conn = 0;
-
-			if (otg->host)
-				otg->host->is_b_host = fsm->id;
-			if (otg->gadget)
-				otg->gadget->is_a_peripheral = !fsm->id;
-			VDBG("ID int (ID is %d)\n", fsm->id);
-
-			if (fsm->id) {	/* switch to gadget */
-				schedule_delayed_work(&((struct fsl_otg *)
-							dev_id)->otg_event,
-							100);
-			} else {	/* switch to host */
-				cancel_delayed_work(&
-						    ((struct fsl_otg *)dev_id)->
-						    otg_event);
-				fsl_otg_start_gadget(fsm, 0);
-				otg_drv_vbus(fsm, 1);
-				fsl_otg_start_host(fsm, 1);
+			fotg->fsm.id = (otg_sc & OTGSC_STS_USB_ID) ? 1 : 0;
+
+			printk("ID int (ID is %d)\n", fotg->fsm.id);
+
+			cancel_delayed_work(&fotg->otg_event);
+			schedule_delayed_work(&fotg->otg_event, msecs_to_jiffies(10));
+			/* if host mode, we should clear B_SESSION_VLD event and disable
+			 * B_SESSION_VLD irq
+			 */
+			if (!fotg->fsm.id) {
+				b_session_irq_enable(false);
+			}else {
+				//b_session_irq_enable(true);
 			}
-
-			return IRQ_HANDLED;
+			ret = IRQ_HANDLED;
 		}
 	}
 
-	return IRQ_NONE;
+	return ret;
+}
+
+static void fsl_otg_fsm_drv_vbus(int on)
+{
+	struct otg_fsm *fsm = &(fsl_otg_dev->fsm);
+	struct otg_transceiver *xceiv = fsm->transceiver;
+
+	struct device *dev;
+	/*
+	 * The host is assigned at otg_set_host
+	 */
+	if (!xceiv->host)
+		return;
+	/*
+	 * The dev is assigned at usb_create_hcd which is called earlier
+	 * than otg_set_host at host driver's probe
+	 */
+	dev = xceiv->host->controller;
+	fsl_otg_drv_vbus(dev->platform_data, on);
 }
 
 static struct otg_fsm_ops fsl_otg_ops = {
 	.chrg_vbus = fsl_otg_chrg_vbus,
-	.drv_vbus = fsl_otg_drv_vbus,
+	.drv_vbus = fsl_otg_fsm_drv_vbus,
 	.loc_conn = fsl_otg_loc_conn,
 	.loc_sof = fsl_otg_loc_sof,
 	.start_pulse = fsl_otg_start_pulse,
@@ -837,6 +913,7 @@ static int fsl_otg_conf(struct platform_device *pdev)
 	fsl_otg_tc->otg.set_power = fsl_otg_set_power;
 	fsl_otg_tc->otg.start_hnp = fsl_otg_start_hnp;
 	fsl_otg_tc->otg.start_srp = fsl_otg_start_srp;
+	fsl_otg_tc->otg.dev = &pdev->dev;
 
 	fsl_otg_dev = fsl_otg_tc;
 
@@ -903,6 +980,8 @@ int usb_otg_start(struct platform_device *pdev)
 	if (pdata->platform_init && pdata->platform_init(pdev) != 0)
 		return -EINVAL;
 
+	clk_stopped = false; /* platform_init will open the otg clk */
+
 	/* stop the controller */
 	temp = readl(&p_otg->dr_mem_map->usbcmd);
 	temp &= ~USB_CMD_RUN_STOP;
@@ -1229,6 +1308,10 @@ static int __init fsl_otg_probe(struct platform_device *pdev)
 		return -EIO;
 	}
 
+	last_busy  = jiffies;
+	setup_timer(&monitor_timer, fsl_otg_loading_monitor, (unsigned long)pdev);
+	mod_timer(&monitor_timer, jiffies + msecs_to_jiffies(TIMER_FREQ));
+
 	create_proc_file();
 	return status;
 }
diff --git a/drivers/video/modedb.c b/drivers/video/modedb.c
index 34e4e7995169..9c4739c8b19c 100644
--- a/drivers/video/modedb.c
+++ b/drivers/video/modedb.c
@@ -35,6 +35,11 @@ EXPORT_SYMBOL_GPL(fb_mode_option);
      */
 
 static const struct fb_videomode modedb[] = {
+	    {
+		/* 800x480 @ 60 Hz, 31.5 kHz hsync */
+	    "LQ070Y3DG3B", 60, 800, 480, 44000, 0, 50, 25, 10, 128, 10,
+	    FB_SYNC_EXT,FB_VMODE_NONINTERLACED
+	    },
     {
 	/* 640x400 @ 70 Hz, 31.5 kHz hsync */
 	NULL, 70, 640, 400, 39721, 40, 24, 39, 9, 96, 2,
diff --git a/drivers/video/mxc/Kconfig b/drivers/video/mxc/Kconfig
index 268879626fc2..42f990ac3499 100644
--- a/drivers/video/mxc/Kconfig
+++ b/drivers/video/mxc/Kconfig
@@ -26,12 +26,21 @@ config FB_MXC_EPSON_VGA_SYNC_PANEL
 
 config FB_MXC_TVOUT_TVE
 	tristate "MXC TVE TV Out Encoder"
-	depends on FB_MXC_SYNC_PANEL
-	depends on MXC_IPU_V3
+        depends on FB_MXC_SYNC_PANEL
+        depends on MXC_IPU_V3
+
+config FB_MXC_LDB
+        tristate "MXC LDB"
+        depends on FB_MXC_SYNC_PANEL
+        depends on MXC_IPU_V3
 
 config FB_MXC_CLAA_WVGA_SYNC_PANEL
+        depends on FB_MXC_SYNC_PANEL
+        tristate "CLAA WVGA Panel"
+
+config FB_MXC_SII9022
 	depends on FB_MXC_SYNC_PANEL
-	tristate "CLAA WVGA Panel"
+	tristate "Si Image SII9022 DVI/HDMI Interface Chip"
 
 config FB_MXC_CH7026
 	depends on FB_MXC_SYNC_PANEL
@@ -43,12 +52,21 @@ config FB_MXC_TVOUT_CH7024
 
 config FB_MXC_LOW_PWR_DISPLAY
 	bool "Low Power Display Refresh Mode"
-	depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
-	default y
+        depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
+        default y
+
+config VIDEO_AD9389
+        tristate "Analog Devices AD9389/AD9889 digital video encoders"
+        depends on I2C && FB_MXC_SYNC_PANEL
+        ---help---
+          Support for the AD9389/AD9889 HDMI/DVI Video transmiter.
+
+          To compile this driver as a module, choose M here: the
+          module will be called ad9389.
 
 config FB_MXC_INTERNAL_MEM
-	bool "Framebuffer in Internal RAM"
-	depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
+        bool "Framebuffer in Internal RAM"
+        depends on FB_MXC_SYNC_PANEL && MXC_FB_IRAM
 	default y
 
 config FB_MXC_ASYNC_PANEL
@@ -66,9 +84,24 @@ config FB_MXC_EPSON_PANEL
 
 endmenu
 
+config FB_MXC_EINK_PANEL
+        depends on FB_MXC
+        depends on DMA_ENGINE
+        select FB_DEFERRED_IO
+        tristate "E-Ink Panel Framebuffer"
+
+config FB_MXC_EINK_AUTO_UPDATE_MODE
+    bool "E-Ink Auto-update Mode Support"
+    default n
+    depends on FB_MXC_EINK_PANEL
+
+config FB_MXC_ELCDIF_FB
+        depends on FB && ARCH_MXC
+        tristate "Support MXC ELCDIF framebuffer"
+
 choice
-	prompt "Async Panel Interface Type"
-	depends on FB_MXC_ASYNC_PANEL && FB_MXC
+        prompt "Async Panel Interface Type"
+        depends on FB_MXC_ASYNC_PANEL && FB_MXC
 	default FB_MXC_ASYNC_PANEL_IFC_16_BIT
 
 config FB_MXC_ASYNC_PANEL_IFC_8_BIT
diff --git a/drivers/video/mxc/Makefile b/drivers/video/mxc/Makefile
index d2454aac2604..d823ab29f030 100644
--- a/drivers/video/mxc/Makefile
+++ b/drivers/video/mxc/Makefile
@@ -18,5 +18,7 @@ obj-$(CONFIG_FB_MXC_EPSON_VGA_SYNC_PANEL)   += mxcfb_epson_vga.o
 obj-$(CONFIG_FB_MXC_CLAA_WVGA_SYNC_PANEL)   += mxcfb_claa_wvga.o
 obj-$(CONFIG_FB_MXC_TVOUT_CH7024)           += ch7024.o
 obj-$(CONFIG_FB_MXC_TVOUT_TVE)              += tve.o
-obj-$(CONFIG_FB_MXC_CH7026)		    		+= mxcfb_ch7026.o
-#obj-$(CONFIG_FB_MODE_HELPERS)				+= mxc_edid.o
+obj-$(CONFIG_FB_MXC_CH7026)                             += mxcfb_ch7026.o
+#obj-$(CONFIG_FB_MODE_HELPERS)                           += mxc_edid.o
+obj-$(CONFIG_VIDEO_AD9389)                      += ad9389.o
+obj-$(CONFIG_FB_MXC_SII9022)                            += mxcfb_sii9022.o
diff --git a/drivers/video/mxc/ccwmx51_display.c b/drivers/video/mxc/ccwmx51_display.c
index 70a5b25f3fe9..212f59e919e4 100755
--- a/drivers/video/mxc/ccwmx51_display.c
+++ b/drivers/video/mxc/ccwmx51_display.c
@@ -22,23 +22,37 @@
 #include <mach/hardware.h>
 #include <mach/mxc.h>
 
+#define MAX_DISPLAYS	2
+#define DISP0_ID	"DISP3 BG"
+#define DISP1_ID	"DISP3 BG - DI1"
+
 static void lcd_poweron(struct ccwmx51_lcd_pdata *plat);
 static void lcd_poweroff(struct ccwmx51_lcd_pdata *plat);
 
-static struct platform_device *plcd_dev;
+static struct platform_device *plcd_dev[MAX_DISPLAYS] = {NULL, NULL};
+
+static int lcd_get_index(struct fb_info *info)
+{
+	if (!strcmp(info->fix.id, DISP0_ID))
+		return 0;
+	else if (!strcmp(info->fix.id, DISP1_ID))
+		return 1;
+	return -1;
+}
 
 static void lcd_init_fb(struct fb_info *info)
 {
-	struct ccwmx51_lcd_pdata *plat = plcd_dev->dev.platform_data;
 	struct fb_var_screeninfo var;
+	struct ccwmx51_lcd_pdata *plat;
+	int i = lcd_get_index(info);
 
-	memset(&var, 0, sizeof(var));
+	if (i < 0)
+		return;
 
+	plat = plcd_dev[i]->dev.platform_data;
+	memset(&var, 0, sizeof(var));
 	fb_videomode_to_var(&var, plat->fb_pdata.mode);
-
 	var.activate = FB_ACTIVATE_ALL;
-	var.yres_virtual = var.yres;
-
 	acquire_console_sem();
 	info->flags |= FBINFO_MISC_USEREVENT;
 	fb_set_var(info, &var);
@@ -74,18 +88,37 @@ static struct notifier_block nb = {
 static int __devinit lcd_sync_probe(struct platform_device *pdev)
 {
 	struct ccwmx51_lcd_pdata *plat = pdev->dev.platform_data;
+	int i;
+
+	if (!plat)
+		return -ENODEV;
 
- 	if (!plat)
- 		return -ENODEV;
+	if (plat->vif < 0 || plat->vif > (MAX_DISPLAYS - 1))
+		return -EINVAL;
 
-	if (plat->reset)
-		plat->reset();
+	if (plat->init)
+		plat->init(plat->vif);
 
-	plcd_dev = pdev;
-	lcd_init_fb(registered_fb[plat->vif]);
-	fb_show_logo(registered_fb[plat->vif], 0);
-	fb_register_client(&nb);
+	plcd_dev[plat->vif] = pdev;
 
+	for (i = 0; i < num_registered_fb; i++) {
+		if ((!strcmp(registered_fb[i]->fix.id, DISP0_ID) && plat->vif == 0) ||
+		    (!strcmp(registered_fb[i]->fix.id, DISP1_ID) && plat->vif == 1)) {
+			lcd_init_fb(registered_fb[i]);
+			/* Clear the screen */
+			memset((char *)registered_fb[i]->screen_base, 0,
+			       registered_fb[i]->fix.smem_len);
+			fb_show_logo(registered_fb[i], 0);
+		}
+	}
+
+	/**
+	 * Register the block notifier only once. The device being notified can be
+	 * retrieved from the received event. There are some issues when the same
+	 * notifier is registered multiple times.
+	 */
+	if (plcd_dev[0] == NULL && plcd_dev[1] == NULL)
+		fb_register_client(&nb);
 	lcd_poweron(plat);
 
 	return 0;
@@ -95,9 +128,14 @@ static int __devexit lcd_sync_remove(struct platform_device *pdev)
 {
 	struct ccwmx51_lcd_pdata *plat = pdev->dev.platform_data;
 
-	fb_unregister_client(&nb);
 	lcd_poweroff(plat);
-	plcd_dev = NULL;
+	if (plat->deinit)
+		plat->deinit(plat->vif);
+
+	plcd_dev[plat->vif] = NULL;
+
+	if (plcd_dev[0] == NULL && plcd_dev[1] == NULL)
+		fb_unregister_client(&nb);
 
 	return 0;
 }
@@ -129,14 +167,14 @@ static struct platform_driver lcd_driver = {
 
 static void lcd_poweron(struct ccwmx51_lcd_pdata *plat)
 {
-	if (plat && plat->bl_enable)
-		plat->bl_enable(0);
+        if (plat && plat->bl_enable)
+                plat->bl_enable(1, plat->vif);
 }
 
 static void lcd_poweroff(struct ccwmx51_lcd_pdata *plat)
 {
-	if (plat && plat->bl_enable)
-		plat->bl_enable(1);
+        if (plat && plat->bl_enable)
+                plat->bl_enable(0, plat->vif);
 }
 
 static int __init lcd_sync_init(void)
diff --git a/drivers/video/mxc/mxc_ipuv3_fb.c b/drivers/video/mxc/mxc_ipuv3_fb.c
index 76c6d523c291..cc48e62637ca 100644
--- a/drivers/video/mxc/mxc_ipuv3_fb.c
+++ b/drivers/video/mxc/mxc_ipuv3_fb.c
@@ -22,7 +22,6 @@
  *
  * @ingroup Framebuffer
  */
-
 /*!
  * Include files
  */
@@ -44,9 +43,13 @@
 #include <linux/io.h>
 #include <linux/ipu.h>
 #include <linux/mxcfb.h>
+#include <linux/earlysuspend.h>
 #include <asm/mach-types.h>
 #include <asm/uaccess.h>
 #include <mach/hardware.h>
+#include <linux/suspend.h>
+
+static int vt_switch;
 
 /*
  * Driver name
@@ -56,11 +59,14 @@
  * Structure containing the MXC specific framebuffer information.
  */
 struct mxcfb_info {
+	char *fb_mode_str;
+	int default_bpp;
 	int cur_blank;
 	int next_blank;
 	ipu_channel_t ipu_ch;
 	int ipu_di;
 	u32 ipu_di_pix_fmt;
+	bool ipu_ext_clk;
 	bool overlay;
 	bool alpha_chan_en;
 	dma_addr_t alpha_phy_addr0;
@@ -74,6 +80,8 @@ struct mxcfb_info {
 
 	u32 pseudo_palette[16];
 
+	bool wait4vsync;
+	uint32_t waitcnt;
 	struct semaphore flip_sem;
 	struct semaphore alpha_flip_sem;
 	struct completion vsync_complete;
@@ -93,12 +101,10 @@ enum {
 	BOTH_OFF
 };
 
-static char *fb_mode;
-static unsigned long default_bpp = 16;
+#define FB_DEVICE_NUM 3
 static bool g_dp_in_use;
 LIST_HEAD(fb_alloc_list);
-static struct fb_info *mxcfb_info[3];
-static int ext_clk_used;
+static struct fb_info *mxcfb_info[FB_DEVICE_NUM];
 
 static uint32_t bpp_to_pixfmt(struct fb_info *fbi)
 {
@@ -125,6 +131,7 @@ static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id);
 static int mxcfb_blank(int blank, struct fb_info *info);
 static int mxcfb_map_video_memory(struct fb_info *fbi);
 static int mxcfb_unmap_video_memory(struct fb_info *fbi);
+static int mxcfb_option_setup(struct fb_info *info, char *options);
 
 /*
  * Set fixed framebuffer parameters based on variable settings.
@@ -165,30 +172,42 @@ static int _setup_disp_channel1(struct fb_info *fbi)
 
 		for (i = 0; i < num_registered_fb; i++) {
 			mxc_fbi_tmp = (struct mxcfb_info *)
-				(registered_fb[i]->par);
+			    (registered_fb[i]->par);
 			if (mxc_fbi_tmp->ipu_ch == MEM_BG_SYNC) {
-				fbi->var.vmode =
-				registered_fb[i]->var.vmode;
+				fbi->var.vmode = registered_fb[i]->var.vmode;
 				mxc_fbi->ipu_di_pix_fmt =
-				mxc_fbi_tmp->ipu_di_pix_fmt;
+				    mxc_fbi_tmp->ipu_di_pix_fmt;
 				break;
 			}
 		}
 	}
-	if (fbi->var.vmode & FB_VMODE_INTERLACED) {
-		params.mem_dp_bg_sync.interlaced = true;
-		params.mem_dp_bg_sync.out_pixel_fmt =
-			IPU_PIX_FMT_YUV444;
+	if (mxc_fbi->ipu_ch == MEM_DC_SYNC) {
+		if (fbi->var.vmode & FB_VMODE_INTERLACED) {
+			params.mem_dc_sync.interlaced = true;
+			params.mem_dc_sync.out_pixel_fmt =
+				IPU_PIX_FMT_YUV444;
+		} else {
+			if (mxc_fbi->ipu_di_pix_fmt)
+				params.mem_dc_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
+			else
+				params.mem_dc_sync.out_pixel_fmt = IPU_PIX_FMT_RGB666;
+		}
+		params.mem_dc_sync.in_pixel_fmt = bpp_to_pixfmt(fbi);
 	} else {
-		if (mxc_fbi->ipu_di_pix_fmt)
-			params.mem_dp_bg_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
-		else
-			params.mem_dp_bg_sync.out_pixel_fmt = IPU_PIX_FMT_RGB666;
+		if (fbi->var.vmode & FB_VMODE_INTERLACED) {
+			params.mem_dp_bg_sync.interlaced = true;
+			params.mem_dp_bg_sync.out_pixel_fmt =
+				IPU_PIX_FMT_YUV444;
+		} else {
+			if (mxc_fbi->ipu_di_pix_fmt)
+				params.mem_dp_bg_sync.out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
+			else
+				params.mem_dp_bg_sync.out_pixel_fmt = IPU_PIX_FMT_RGB666;
+		}
+		params.mem_dp_bg_sync.in_pixel_fmt = bpp_to_pixfmt(fbi);
+		if (mxc_fbi->alpha_chan_en)
+			params.mem_dp_bg_sync.alpha_chan_en = true;
 	}
-	params.mem_dp_bg_sync.in_pixel_fmt = bpp_to_pixfmt(fbi);
-	if (mxc_fbi->alpha_chan_en)
-		params.mem_dp_bg_sync.alpha_chan_en = true;
-
 	ipu_init_channel(mxc_fbi->ipu_ch, &params);
 
 	return 0;
@@ -201,10 +220,12 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 	int fb_stride;
 
 	switch (bpp_to_pixfmt(fbi)) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-	case V4L2_PIX_FMT_NV12:
-	case V4L2_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YUV420P2:
+	case IPU_PIX_FMT_YVU420P:
+	case IPU_PIX_FMT_NV12:
+	case IPU_PIX_FMT_YUV422P:
+	case IPU_PIX_FMT_YVU422P:
+	case IPU_PIX_FMT_YUV420P:
 		fb_stride = fbi->var.xres_virtual;
 		break;
 	default:
@@ -212,7 +233,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 	}
 
 	mxc_fbi->cur_ipu_buf = 1;
-	sema_init(&mxc_fbi->flip_sem, 1);
+	sema_init(&mxc_fbi->flip_sem, 0);
 	if (mxc_fbi->alpha_chan_en) {
 		mxc_fbi->cur_ipu_alpha_buf = 1;
 		sema_init(&mxc_fbi->alpha_flip_sem, 1);
@@ -226,8 +247,7 @@ static int _setup_disp_channel2(struct fb_info *fbi)
 					 IPU_ROTATE_NONE,
 					 fbi->fix.smem_start +
 					 (fbi->fix.line_length * fbi->var.yres),
-					 fbi->fix.smem_start,
-					 0, 0);
+					 fbi->fix.smem_start, 0, 0);
 	if (retval) {
 		dev_err(fbi->device,
 			"ipu_init_channel_buffer error %d\n", retval);
@@ -297,38 +317,47 @@ static int mxcfb_set_par(struct fb_info *fbi)
 						  mxc_fbi->alpha_phy_addr1);
 
 			mxc_fbi->alpha_virt_addr0 =
-					dma_alloc_coherent(fbi->device,
-						  alpha_mem_len,
-						  &mxc_fbi->alpha_phy_addr0,
-						  GFP_DMA | GFP_KERNEL);
+			    dma_alloc_coherent(fbi->device,
+					       alpha_mem_len,
+					       &mxc_fbi->alpha_phy_addr0,
+					       GFP_DMA | GFP_KERNEL);
 
 			mxc_fbi->alpha_virt_addr1 =
-					dma_alloc_coherent(fbi->device,
-						  alpha_mem_len,
-						  &mxc_fbi->alpha_phy_addr1,
-						  GFP_DMA | GFP_KERNEL);
+			    dma_alloc_coherent(fbi->device,
+					       alpha_mem_len,
+					       &mxc_fbi->alpha_phy_addr1,
+					       GFP_DMA | GFP_KERNEL);
 			if (mxc_fbi->alpha_virt_addr0 == NULL ||
 			    mxc_fbi->alpha_virt_addr1 == NULL) {
 				dev_err(fbi->device, "mxcfb: dma alloc for"
 					" alpha buffer failed.\n");
 				if (mxc_fbi->alpha_virt_addr0)
 					dma_free_coherent(fbi->device,
-						  mxc_fbi->alpha_mem_len,
-						  mxc_fbi->alpha_virt_addr0,
-						  mxc_fbi->alpha_phy_addr0);
+							  mxc_fbi->
+							  alpha_mem_len,
+							  mxc_fbi->
+							  alpha_virt_addr0,
+							  mxc_fbi->
+							  alpha_phy_addr0);
 				if (mxc_fbi->alpha_virt_addr1)
 					dma_free_coherent(fbi->device,
-						  mxc_fbi->alpha_mem_len,
-						  mxc_fbi->alpha_virt_addr1,
-						  mxc_fbi->alpha_phy_addr1);
+							  mxc_fbi->
+							  alpha_mem_len,
+							  mxc_fbi->
+							  alpha_virt_addr1,
+							  mxc_fbi->
+							  alpha_phy_addr1);
 				return -ENOMEM;
 			}
 			mxc_fbi->alpha_mem_len = alpha_mem_len;
 		}
 	}
 
+#if !(defined(CONFIG_CCWMX51_DISP0) && defined(CONFIG_CCWMX51_DISP1))
+	/* FIXME this lines of code doesnt allow to run the dual head... */
 	if (mxc_fbi->next_blank != FB_BLANK_UNBLANK)
 		return retval;
+#endif
 
 	_setup_disp_channel1(fbi);
 
@@ -345,9 +374,9 @@ static int mxcfb_set_par(struct fb_info *fbi)
 			else
 				out_pixel_fmt = IPU_PIX_FMT_RGB666;
 		}
-		if (fbi->var.vmode & FB_VMODE_ODD_FLD_FIRST) /* PAL */
+		if (fbi->var.vmode & FB_VMODE_ODD_FLD_FIRST)	/* PAL */
 			sig_cfg.odd_field_first = true;
-		if ((fbi->var.sync & FB_SYNC_EXT) || ext_clk_used)
+		if ((fbi->var.sync & FB_SYNC_EXT) || mxc_fbi->ipu_ext_clk)
 			sig_cfg.ext_clk = true;
 		if (fbi->var.sync & FB_SYNC_HOR_HIGH_ACT)
 			sig_cfg.Hsync_pol = true;
@@ -453,8 +482,7 @@ static int swap_channels(struct fb_info *fbi)
 		ch_to = MEM_BG_SYNC;
 
 	for (i = 0; i < num_registered_fb; i++) {
-		mxc_fbi_to =
-			(struct mxcfb_info *)mxcfb_info[i]->par;
+		mxc_fbi_to = (struct mxcfb_info *)mxcfb_info[i]->par;
 		if (mxc_fbi_to->ipu_ch == ch_to) {
 			fbi_to = mxcfb_info[i];
 			break;
@@ -519,14 +547,14 @@ static int swap_channels(struct fb_info *fbi)
 	}
 
 	if (ipu_request_irq(mxc_fbi_from->ipu_ch_irq, mxcfb_irq_handler, 0,
-		MXCFB_NAME, fbi) != 0) {
+			    MXCFB_NAME, fbi) != 0) {
 		dev_err(fbi->device, "Error registering irq %d\n",
 			mxc_fbi_from->ipu_ch_irq);
 		return -EBUSY;
 	}
 	ipu_disable_irq(mxc_fbi_from->ipu_ch_irq);
 	if (ipu_request_irq(mxc_fbi_to->ipu_ch_irq, mxcfb_irq_handler, 0,
-		MXCFB_NAME, fbi_to) != 0) {
+			    MXCFB_NAME, fbi_to) != 0) {
 		dev_err(fbi_to->device, "Error registering irq %d\n",
 			mxc_fbi_to->ipu_ch_irq);
 		return -EBUSY;
@@ -545,17 +573,48 @@ static int swap_channels(struct fb_info *fbi)
  */
 static int mxcfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
+	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par;
 	u32 vtotal;
 	u32 htotal;
 
+	/* fg should not bigger than bg */
+	if (mxc_fbi->ipu_ch == MEM_FG_SYNC) {
+		struct fb_info *fbi_tmp;
+		struct mxcfb_info *mxc_fbi_tmp;
+		int i, bg_xres, bg_yres;
+		int16_t pos_x, pos_y;
+
+		bg_xres = var->xres;
+		bg_yres = var->yres;
+
+		for (i = 0; i < num_registered_fb; i++) {
+			fbi_tmp = registered_fb[i];
+			mxc_fbi_tmp = (struct mxcfb_info *)
+				(fbi_tmp->par);
+			if (mxc_fbi_tmp->ipu_ch == MEM_BG_SYNC) {
+				bg_xres = fbi_tmp->var.xres;
+				bg_yres = fbi_tmp->var.yres;
+				break;
+			}
+		}
+
+		ipu_disp_get_window_pos(mxc_fbi->ipu_ch, &pos_x, &pos_y);
+
+		if ((var->xres + pos_x) > bg_xres)
+			var->xres = bg_xres - pos_x;
+		if ((var->yres + pos_y) > bg_yres)
+			var->yres = bg_yres - pos_y;
+	}
+
 	if (var->xres_virtual < var->xres)
 		var->xres_virtual = var->xres;
 	if (var->yres_virtual < var->yres)
 		var->yres_virtual = var->yres;
 
 	if ((var->bits_per_pixel != 32) && (var->bits_per_pixel != 24) &&
-	    (var->bits_per_pixel != 16) && (var->bits_per_pixel != 8))
-		var->bits_per_pixel = default_bpp;
+	    (var->bits_per_pixel != 16) && (var->bits_per_pixel != 12) &&
+	    (var->bits_per_pixel != 8))
+		var->bits_per_pixel = mxc_fbi->default_bpp;
 
 	switch (var->bits_per_pixel) {
 	case 8:
@@ -723,7 +782,7 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 			}
 
 			if (ipu_disp_set_global_alpha(mxc_fbi->ipu_ch,
-						      (bool)ga.enable,
+						      (bool) ga.enable,
 						      ga.alpha)) {
 				retval = -EINVAL;
 				break;
@@ -750,7 +809,7 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 			}
 
 			if (ipu_disp_set_global_alpha(mxc_fbi->ipu_ch,
-						      !(bool)la.enable, 0)) {
+						      !(bool) la.enable, 0)) {
 				retval = -EINVAL;
 				break;
 			}
@@ -765,8 +824,11 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
 				for (i = 0; i < num_registered_fb; i++) {
 					char *idstr = registered_fb[i]->fix.id;
-					if (strcmp(idstr, video_plane_idstr) == 0) {
-						((struct mxcfb_info *)(registered_fb[i]->par))->alpha_chan_en = false;
+					if (strcmp(idstr, video_plane_idstr) ==
+					    0) {
+						((struct mxcfb_info
+						  *)(registered_fb[i]->par))->
+				  alpha_chan_en = false;
 						break;
 					}
 				}
@@ -794,8 +856,8 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 			uint32_t ipu_alp_ch_irq;
 
 			if (!(((mxc_fbi->ipu_ch == MEM_FG_SYNC) ||
-			     (mxc_fbi->ipu_ch == MEM_BG_SYNC)) &&
-			     (mxc_fbi->alpha_chan_en))) {
+			       (mxc_fbi->ipu_ch == MEM_BG_SYNC)) &&
+			      (mxc_fbi->alpha_chan_en))) {
 				dev_err(fbi->device,
 					"Should use background or overlay "
 					"framebuffer to set the alpha buffer "
@@ -822,11 +884,10 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 			down(&mxc_fbi->alpha_flip_sem);
 
 			mxc_fbi->cur_ipu_alpha_buf =
-						!mxc_fbi->cur_ipu_alpha_buf;
+			    !mxc_fbi->cur_ipu_alpha_buf;
 			if (ipu_update_channel_buffer(mxc_fbi->ipu_ch,
 						      IPU_ALPHA_IN_BUFFER,
-						      mxc_fbi->
-							cur_ipu_alpha_buf,
+						      mxc_fbi->cur_ipu_alpha_buf,
 						      base) == 0) {
 				ipu_select_buffer(mxc_fbi->ipu_ch,
 						  IPU_ALPHA_IN_BUFFER,
@@ -864,9 +925,9 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 				break;
 			}
 			retval = ipu_disp_set_gamma_correction(mxc_fbi->ipu_ch,
-							gamma.enable,
-							gamma.constk,
-							gamma.slopek);
+							       gamma.enable,
+							       gamma.constk,
+							       gamma.slopek);
 			break;
 		}
 	case MXCFB_WAIT_FOR_VSYNC:
@@ -876,7 +937,8 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 				int i;
 				for (i = 0; i < num_registered_fb; i++) {
 					bg_mxcfbi =
-					((struct mxcfb_info *)(registered_fb[i]->par));
+					    ((struct mxcfb_info
+					      *)(registered_fb[i]->par));
 
 					if (bg_mxcfbi->ipu_ch == MEM_BG_SYNC)
 						break;
@@ -891,21 +953,25 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 				break;
 			}
 
-			down(&mxc_fbi->flip_sem);
 			init_completion(&mxc_fbi->vsync_complete);
 
 			ipu_clear_irq(mxc_fbi->ipu_ch_irq);
+			mxc_fbi->wait4vsync = 1;
 			ipu_enable_irq(mxc_fbi->ipu_ch_irq);
-			retval = wait_for_completion_interruptible_timeout(
-				&mxc_fbi->vsync_complete, 1 * HZ);
+			retval =
+			    wait_for_completion_interruptible_timeout(&mxc_fbi->
+								      vsync_complete,
+								      1 * HZ);
 			if (retval == 0) {
 				dev_err(fbi->device,
 					"MXCFB_WAIT_FOR_VSYNC: timeout %d\n",
 					retval);
+				mxc_fbi->wait4vsync = 0;
 				retval = -ETIME;
 			} else if (retval > 0) {
 				retval = 0;
 			}
+
 			break;
 		}
 	case FBIO_ALLOC:
@@ -986,7 +1052,8 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 
 			for (i = 0; i < num_registered_fb; i++) {
 				bg_mxcfbi =
-				((struct mxcfb_info *)(registered_fb[i]->par));
+				    ((struct mxcfb_info *)(registered_fb[i]->
+							   par));
 
 				if (bg_mxcfbi->ipu_ch == MEM_BG_SYNC) {
 					bg_fbi = registered_fb[i];
@@ -1005,13 +1072,15 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 				if (bg_fbi->var.xres < fbi->var.xres)
 					pos.x = 0;
 				else
-					pos.x = bg_fbi->var.xres - fbi->var.xres;
+					pos.x =
+					    bg_fbi->var.xres - fbi->var.xres;
 			}
 			if (fbi->var.yres + pos.y > bg_fbi->var.yres) {
 				if (bg_fbi->var.yres < fbi->var.yres)
 					pos.y = 0;
 				else
-					pos.y = bg_fbi->var.yres - fbi->var.yres;
+					pos.y =
+					    bg_fbi->var.yres - fbi->var.yres;
 			}
 
 			retval = ipu_disp_set_window_pos(mxc_fbi->ipu_ch,
@@ -1026,12 +1095,39 @@ static int mxcfb_ioctl(struct fb_info *fbi, unsigned int cmd, unsigned long arg)
 	case MXCFB_GET_FB_IPU_CHAN:
 		{
 			struct mxcfb_info *mxc_fbi =
-				(struct mxcfb_info *)fbi->par;
+			    (struct mxcfb_info *)fbi->par;
 
 			if (put_user(mxc_fbi->ipu_ch, argp))
 				return -EFAULT;
 			break;
 		}
+	case MXCFB_GET_DIFMT:
+		{
+			struct mxcfb_info *mxc_fbi =
+			    (struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_di_pix_fmt, argp))
+				return -EFAULT;
+			break;
+		}
+	case MXCFB_GET_FB_IPU_DI:
+		{
+			struct mxcfb_info *mxc_fbi =
+			    (struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->ipu_di, argp))
+				return -EFAULT;
+			break;
+		}
+	case MXCFB_GET_FB_BLANK:
+		{
+			struct mxcfb_info *mxc_fbi =
+				(struct mxcfb_info *)fbi->par;
+
+			if (put_user(mxc_fbi->cur_blank, argp))
+				return -EFAULT;
+			break;
+		}
 	default:
 		retval = -EINVAL;
 	}
@@ -1081,7 +1177,7 @@ static int
 mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)info->par,
-			  *mxc_graphic_fbi = NULL;
+	    *mxc_graphic_fbi = NULL;
 	u_int y_bottom;
 	unsigned long base, active_alpha_phy_addr = 0;
 	bool loc_alpha_en = false;
@@ -1102,7 +1198,7 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		int j;
 		for (j = 0; j < num_registered_fb; j++) {
 			bg_mxcfbi =
-				((struct mxcfb_info *)(registered_fb[j]->par));
+			    ((struct mxcfb_info *)(registered_fb[j]->par));
 
 			if (bg_mxcfbi->ipu_ch == MEM_BG_SYNC)
 				break;
@@ -1122,12 +1218,9 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		return -EINVAL;
 
 	base = (var->yoffset * var->xres_virtual + var->xoffset);
-	base *= (var->bits_per_pixel) / 8;
+	base = (var->bits_per_pixel) * base / 8;
 	base += info->fix.smem_start;
 
-	dev_dbg(info->device, "Updating SDC %s buf %d address=0x%08lX\n",
-		info->fix.id, mxc_fbi->cur_ipu_buf, base);
-
 	/* Check if DP local alpha is enabled and find the graphic fb */
 	if (mxc_fbi->ipu_ch == MEM_BG_SYNC || mxc_fbi->ipu_ch == MEM_FG_SYNC) {
 		for (i = 0; i < num_registered_fb; i++) {
@@ -1135,13 +1228,13 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 			if ((strcmp(idstr, "DISP3 BG") == 0 ||
 			     strcmp(idstr, "DISP3 FG") == 0) &&
 			    ((struct mxcfb_info *)
-			      (registered_fb[i]->par))->alpha_chan_en) {
+			     (registered_fb[i]->par))->alpha_chan_en) {
 				loc_alpha_en = true;
 				mxc_graphic_fbi = (struct mxcfb_info *)
-						(registered_fb[i]->par);
+				    (registered_fb[i]->par);
 				active_alpha_phy_addr = mxc_fbi->cur_ipu_buf ?
-					mxc_graphic_fbi->alpha_phy_addr1 :
-					mxc_graphic_fbi->alpha_phy_addr0;
+				    mxc_graphic_fbi->alpha_phy_addr1 :
+				    mxc_graphic_fbi->alpha_phy_addr0;
 				dev_dbg(info->device, "Updating SDC graphic "
 					"buf %d address=0x%08lX\n",
 					mxc_fbi->cur_ipu_buf,
@@ -1151,10 +1244,13 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		}
 	}
 
-	down(&mxc_fbi->flip_sem);
 	init_completion(&mxc_fbi->vsync_complete);
 
 	mxc_fbi->cur_ipu_buf = !mxc_fbi->cur_ipu_buf;
+
+	dev_dbg(info->device, "Updating SDC %s buf %d address=0x%08lX\n",
+		info->fix.id, mxc_fbi->cur_ipu_buf, base);
+
 	if (ipu_update_channel_buffer(mxc_fbi->ipu_ch, IPU_INPUT_BUFFER,
 				      mxc_fbi->cur_ipu_buf, base) == 0) {
 		/* Update the DP local alpha buffer only for graphic plane */
@@ -1176,8 +1272,13 @@ mxcfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 		dev_err(info->device,
 			"Error updating SDC buf %d to address=0x%08lX\n",
 			mxc_fbi->cur_ipu_buf, base);
+		mxc_fbi->cur_ipu_buf = !mxc_fbi->cur_ipu_buf;
+		ipu_clear_irq(mxc_fbi->ipu_ch_irq);
+		ipu_enable_irq(mxc_fbi->ipu_ch_irq);
+		return -EBUSY;
 	}
 
+	down(&mxc_fbi->flip_sem);
 	dev_dbg(info->device, "Update complete\n");
 
 	info->var.xoffset = var->xoffset;
@@ -1211,9 +1312,9 @@ static int mxcfb_mmap(struct fb_info *fbi, struct vm_area_struct *vma)
 		len = fbi->fix.smem_len - offset;
 		vma->vm_pgoff = (fbi->fix.smem_start + offset) >> PAGE_SHIFT;
 	} else if ((vma->vm_pgoff ==
-			(mxc_fbi->alpha_phy_addr0 >> PAGE_SHIFT)) ||
+		    (mxc_fbi->alpha_phy_addr0 >> PAGE_SHIFT)) ||
 		   (vma->vm_pgoff ==
-			(mxc_fbi->alpha_phy_addr1 >> PAGE_SHIFT))) {
+		    (mxc_fbi->alpha_phy_addr1 >> PAGE_SHIFT))) {
 		len = mxc_fbi->alpha_mem_len;
 	} else {
 		list_for_each_entry(mem, &fb_alloc_list, list) {
@@ -1269,9 +1370,14 @@ static irqreturn_t mxcfb_irq_handler(int irq, void *dev_id)
 	struct fb_info *fbi = dev_id;
 	struct mxcfb_info *mxc_fbi = fbi->par;
 
-	complete(&mxc_fbi->vsync_complete);
-	up(&mxc_fbi->flip_sem);
-	ipu_disable_irq(irq);
+	if (mxc_fbi->wait4vsync) {
+		complete(&mxc_fbi->vsync_complete);
+		ipu_disable_irq(irq);
+		mxc_fbi->wait4vsync = 0;
+	} else {
+		up(&mxc_fbi->flip_sem);
+		ipu_disable_irq(irq);
+	}
 	return IRQ_HANDLED;
 }
 
@@ -1288,14 +1394,10 @@ static irqreturn_t mxcfb_alpha_irq_handler(int irq, void *dev_id)
 /*
  * Suspends the framebuffer and blanks the screen. Power management support
  */
-static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
+static void mxcfb_suspend_one(struct fb_info *fbi)
 {
-	struct fb_info *fbi = platform_get_drvdata(pdev);
 	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
 	int saved_blank;
-#ifdef CONFIG_FB_MXC_LOW_PWR_DISPLAY
-	void *fbmem;
-#endif
 
 	acquire_console_sem();
 	fb_set_suspend(fbi, 1);
@@ -1303,25 +1405,43 @@ static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
 	mxcfb_blank(FB_BLANK_POWERDOWN, fbi);
 	mxc_fbi->next_blank = saved_blank;
 	release_console_sem();
-
-	return 0;
 }
 
 /*
  * Resumes the framebuffer and unblanks the screen. Power management support
  */
-static int mxcfb_resume(struct platform_device *pdev)
+static void mxcfb_resume_one(struct fb_info *fbi)
 {
-	struct fb_info *fbi = platform_get_drvdata(pdev);
 	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
 
 	acquire_console_sem();
 	mxcfb_blank(mxc_fbi->next_blank, fbi);
 	fb_set_suspend(fbi, 0);
 	release_console_sem();
+}
+
+#ifndef CONFIG_HAS_EARLYSUSPEND
+
+static int mxcfb_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+
+	if (fbi && (strcmp(fbi->fix.id, "DISP3 FG") == 0))
+		mxcfb_suspend_one(fbi);
+
+	return 0;
+}
+
+static int mxcfb_resume(struct platform_device *pdev)
+{
+	struct fb_info *fbi = platform_get_drvdata(pdev);
+
+	if (fbi && (strcmp(fbi->fix.id, "DISP3 FG") == 0))
+		mxcfb_resume_one(fbi);
 
 	return 0;
 }
+#endif
 
 /*
  * Main framebuffer functions
@@ -1341,12 +1461,12 @@ static int mxcfb_map_video_memory(struct fb_info *fbi)
 {
 	if (fbi->fix.smem_len < fbi->var.yres_virtual * fbi->fix.line_length)
 		fbi->fix.smem_len = fbi->var.yres_virtual *
-				    fbi->fix.line_length;
+		    fbi->fix.line_length;
 
 	fbi->screen_base = dma_alloc_writecombine(fbi->device,
-				fbi->fix.smem_len,
-				(dma_addr_t *)&fbi->fix.smem_start,
-				GFP_DMA);
+						  fbi->fix.smem_len,
+						  (dma_addr_t *) & fbi->fix.
+						  smem_start, GFP_DMA);
 	if (fbi->screen_base == 0) {
 		dev_err(fbi->device, "Unable to allocate framebuffer memory\n");
 		fbi->fix.smem_len = 0;
@@ -1443,6 +1563,7 @@ static ssize_t swap_disp_chan(struct device *dev,
 	struct mxcfb_info *mxcfbi = (struct mxcfb_info *)info->par;
 	struct mxcfb_info *fg_mxcfbi = NULL;
 
+	acquire_console_sem();
 	/* swap only happen between DP-BG and DC, while DP-FG disable */
 	if (((mxcfbi->ipu_ch == MEM_BG_SYNC) &&
 	     (strstr(buf, "1-layer-fb") != NULL)) ||
@@ -1451,17 +1572,16 @@ static ssize_t swap_disp_chan(struct device *dev,
 		int i;
 
 		for (i = 0; i < num_registered_fb; i++) {
-			fg_mxcfbi =
-				(struct mxcfb_info *)mxcfb_info[i]->par;
+			fg_mxcfbi = (struct mxcfb_info *)mxcfb_info[i]->par;
 			if (fg_mxcfbi->ipu_ch == MEM_FG_SYNC)
 				break;
 			else
 				fg_mxcfbi = NULL;
 		}
-		if (!fg_mxcfbi ||
-			fg_mxcfbi->cur_blank == FB_BLANK_UNBLANK) {
+		if (!fg_mxcfbi || fg_mxcfbi->cur_blank == FB_BLANK_UNBLANK) {
 			dev_err(dev,
 				"Can not switch while fb2(fb-fg) is on.\n");
+			release_console_sem();
 			return count;
 		}
 
@@ -1469,8 +1589,10 @@ static ssize_t swap_disp_chan(struct device *dev,
 			dev_err(dev, "Swap display channel failed.\n");
 	}
 
+	release_console_sem();
 	return count;
 }
+
 DEVICE_ATTR(fsl_disp_property, 644, show_disp_chan, swap_disp_chan);
 
 /*!
@@ -1487,6 +1609,8 @@ static int mxcfb_probe(struct platform_device *pdev)
 	struct mxcfb_info *mxcfbi;
 	struct mxc_fb_platform_data *plat_data = pdev->dev.platform_data;
 	struct resource *res;
+	char *options, *mstr;
+	char name[] = "mxcdi0fb";
 	int ret = 0;
 
 	/*
@@ -1499,6 +1623,13 @@ static int mxcfb_probe(struct platform_device *pdev)
 	}
 	mxcfbi = (struct mxcfb_info *)fbi->par;
 
+	name[5] += pdev->id;
+	if (fb_get_options(name, &options))
+		return -ENODEV;
+
+	if (options)
+		mxcfb_option_setup(fbi, options);
+
 	if (!g_dp_in_use) {
 		mxcfbi->ipu_ch_irq = IPU_IRQ_BG_SYNC_EOF;
 		mxcfbi->ipu_ch = MEM_BG_SYNC;
@@ -1521,7 +1652,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 					    mxcfb_alpha_irq_handler, 0,
 					    MXCFB_NAME, fbi) != 0) {
 				dev_err(&pdev->dev, "Error registering BG "
-						    "alpha irq handler.\n");
+					"alpha irq handler.\n");
 				ret = -EBUSY;
 				goto err1;
 			}
@@ -1534,7 +1665,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 					    mxcfb_alpha_irq_handler, 0,
 					    MXCFB_NAME, fbi) != 0) {
 				dev_err(&pdev->dev, "Error registering BG "
-						    "alpha irq handler.\n");
+					"alpha irq handler.\n");
 				ret = -EBUSY;
 				goto err1;
 			}
@@ -1552,7 +1683,7 @@ static int mxcfb_probe(struct platform_device *pdev)
 				    mxcfb_alpha_irq_handler, 0,
 				    MXCFB_NAME, fbi) != 0) {
 			dev_err(&pdev->dev, "Error registering FG alpha irq "
-					    "handler.\n");
+				"handler.\n");
 			ret = -EBUSY;
 			goto err1;
 		}
@@ -1572,31 +1703,63 @@ static int mxcfb_probe(struct platform_device *pdev)
 	if (res && res->end) {
 		fbi->fix.smem_len = res->end - res->start + 1;
 		fbi->fix.smem_start = res->start;
-		fbi->screen_base = ioremap(fbi->fix.smem_start, fbi->fix.smem_len);
+		fbi->screen_base =
+		    ioremap(fbi->fix.smem_start, fbi->fix.smem_len);
 	}
 
 	/* Need dummy values until real panel is configured */
 	fbi->var.xres = 240;
 	fbi->var.yres = 320;
 
-	if (!fb_mode && plat_data && plat_data->mode_str)
-		fb_find_mode(&fbi->var, fbi, plat_data->mode_str, NULL, 0, NULL,
-			     default_bpp);
-
-	if (fb_mode)
-		fb_find_mode(&fbi->var, fbi, fb_mode, NULL, 0, NULL,
-			     default_bpp);
+	if (!mxcfbi->default_bpp)
+#ifdef CONFIG_CCWMX51_DEFAULT_VIDEO_BPP
+		mxcfbi->default_bpp = CONFIG_CCWMX51_DEFAULT_VIDEO_BPP;
+#else
+		mxcfbi->default_bpp = 16;
+#endif
 
-	if (plat_data) {
+	if (plat_data && !mxcfbi->ipu_di_pix_fmt)
 		mxcfbi->ipu_di_pix_fmt = plat_data->interface_pix_fmt;
-		if (!fb_mode && plat_data->mode)
-			fb_videomode_to_var(&fbi->var, plat_data->mode);
+
+	if (plat_data && plat_data->mode && plat_data->num_modes)
+		fb_videomode_to_modelist(plat_data->mode, plat_data->num_modes,
+				&fbi->modelist);
+
+	if (!mxcfbi->fb_mode_str && plat_data && plat_data->mode_str)
+		mxcfbi->fb_mode_str = plat_data->mode_str;
+
+	if (mxcfbi->fb_mode_str) {
+
+#ifdef CONFIG_MODULE_CCXMX51
+		if ((mstr = strstr(mxcfbi->fb_mode_str, "VGA@")) != NULL)
+			mxcfbi->fb_mode_str = mstr + 4;
+#endif
+
+		ret =
+		    fb_find_mode(&fbi->var, fbi, mxcfbi->fb_mode_str, NULL, 0,
+				 NULL, mxcfbi->default_bpp);
+		if ((!ret || (ret > 2)) && plat_data && plat_data->mode
+		    && plat_data->num_modes)
+			fb_find_mode(&fbi->var, fbi, mxcfbi->fb_mode_str,
+				     plat_data->mode, plat_data->num_modes,
+				     NULL, mxcfbi->default_bpp);
+
+#ifdef CONFIG_MODULE_CCXMX51
+		/* This improves the VGA modes on the CCWi-i.MX51 */
+		if (mstr != NULL) {
+			mxcfbi->ipu_ext_clk = true;
+			fbi->var.sync |= FB_SYNC_CLK_LAT_FALL;
+		}
+#endif
 	}
 
 	mxcfb_check_var(&fbi->var, fbi);
 
+	pm_set_vt_switch(vt_switch);
+
 	/* Default Y virtual size is 2x panel size */
-	fbi->var.yres_virtual = fbi->var.yres * 3;
+	fbi->var.yres_virtual = ((fbi->var.yres + 127) & ~127) * 2;
+	fbi->var.xres_virtual = (fbi->var.xres + 31) & ~31;
 
 	mxcfb_set_fix(fbi);
 
@@ -1617,12 +1780,12 @@ static int mxcfb_probe(struct platform_device *pdev)
 
 	return 0;
 
-err2:
+      err2:
 	ipu_free_irq(mxcfbi->ipu_ch_irq, fbi);
-err1:
+      err1:
 	fb_dealloc_cmap(&fbi->cmap);
 	framebuffer_release(fbi);
-err0:
+      err0:
 	return ret;
 }
 
@@ -1655,38 +1818,108 @@ static struct platform_driver mxcfb_driver = {
 		   },
 	.probe = mxcfb_probe,
 	.remove = mxcfb_remove,
+#ifndef CONFIG_HAS_EARLYSUSPEND
 	.suspend = mxcfb_suspend,
 	.resume = mxcfb_resume,
+#endif
 };
 
 /*
  * Parse user specified options (`video=trident:')
  * example:
- * 	video=trident:800x600,bpp=16,noaccel
+ * 	video=mxcdi0fb:RGB24, 1024x768M-16@60,bpp=16,noaccel
  */
-int mxcfb_setup(char *options)
+static int mxcfb_option_setup(struct fb_info *info, char *options)
 {
+	struct mxcfb_info *mxcfbi = info->par;
 	char *opt;
+
 	if (!options || !*options)
 		return 0;
+
 	while ((opt = strsep(&options, ",")) != NULL) {
 		if (!*opt)
 			continue;
+
+		if (!strncmp(opt, "RGB24", 5)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_RGB24;
+			continue;
+		}
+		if (!strncmp(opt, "BGR24", 5)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_BGR24;
+			continue;
+		}
+		if (!strncmp(opt, "RGB565", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_RGB565;
+			continue;
+		}
+		if (!strncmp(opt, "RGB666", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_RGB666;
+			continue;
+		}
+		if (!strncmp(opt, "YUV444", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_YUV444;
+			continue;
+		}
+		if (!strncmp(opt, "LVDS666", 7)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_LVDS666;
+			continue;
+		}
+		if (!strncmp(opt, "YUYV16", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_YUYV;
+			continue;
+		}
+		if (!strncmp(opt, "UYVY16", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_UYVY;
+			continue;
+		}
+		if (!strncmp(opt, "YVYU16", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_YVYU;
+			continue;
+		}
+		if (!strncmp(opt, "VYUY16", 6)) {
+			mxcfbi->ipu_di_pix_fmt = IPU_PIX_FMT_VYUY;
+			continue;
+		}
 		if (!strncmp(opt, "ext_clk", 7)) {
-			ext_clk_used = true;
+			mxcfbi->ipu_ext_clk = true;
 			continue;
-		} else
-			ext_clk_used = false;
-
+		}
 		if (!strncmp(opt, "bpp=", 4))
-			default_bpp = simple_strtoul(opt + 4, NULL, 0);
+			mxcfbi->default_bpp = simple_strtoul(opt + 4, NULL, 0);
 		else
-			fb_mode = opt;
-
+			mxcfbi->fb_mode_str = opt;
 	}
+
 	return 0;
 }
 
+#ifdef CONFIG_HAS_EARLYSUSPEND
+static void mxcfb_early_suspend(struct early_suspend *h)
+{
+	int i;
+	for (i = FB_DEVICE_NUM - 1; i >= 0; i--) {
+		if (mxcfb_info[i])
+			mxcfb_suspend_one(mxcfb_info[i]);
+	}
+}
+
+static void mxcfb_later_resume(struct early_suspend *h)
+{
+	int i;
+	for (i = 0; i < FB_DEVICE_NUM; i++) {
+		if (mxcfb_info[i])
+			mxcfb_resume_one(mxcfb_info[i]);
+	}
+}
+
+struct early_suspend fbdrv_earlysuspend = {
+	.level = EARLY_SUSPEND_LEVEL_DISABLE_FB,
+	.suspend = mxcfb_early_suspend,
+	.resume = mxcfb_later_resume,
+};
+#endif
+
 /*!
  * Main entry function for the framebuffer. The function registers the power
  * management callback functions with the kernel and also registers the MXCFB
@@ -1696,26 +1929,23 @@ int mxcfb_setup(char *options)
  */
 int __init mxcfb_init(void)
 {
-	int ret = 0;
-#ifndef MODULE
-	char *option = NULL;
-#endif
-
-#ifndef MODULE
-	if (fb_get_options("mxcfb", &option))
-		return -ENODEV;
-	mxcfb_setup(option);
-#endif
+	int ret;
 
 	ret = platform_driver_register(&mxcfb_driver);
+	if (!ret)
+		register_early_suspend(&fbdrv_earlysuspend);
 	return ret;
 }
 
 void mxcfb_exit(void)
 {
+	unregister_early_suspend(&fbdrv_earlysuspend);
 	platform_driver_unregister(&mxcfb_driver);
 }
 
+module_param(vt_switch, int, 0);
+MODULE_PARM_DESC(vt_switch, "enable VT switch during suspend/resume");
+
 module_init(mxcfb_init);
 module_exit(mxcfb_exit);
 
diff --git a/drivers/video/mxc/mxcfb_claa_wvga.c b/drivers/video/mxc/mxcfb_claa_wvga.c
index bd5ff7b83f8c..8f696c19e7d9 100644
--- a/drivers/video/mxc/mxcfb_claa_wvga.c
+++ b/drivers/video/mxc/mxcfb_claa_wvga.c
@@ -48,8 +48,8 @@ static int lcd_on;
 
 static struct fb_videomode video_modes[] = {
 	{
-	 /* 800x480 @ 55 Hz , pixel clk @ 25MHz */
-	 "CLAA-WVGA", 55, 800, 480, 40000, 40, 40, 5, 5, 20, 10,
+	 /* 800x480 @ 57 Hz , pixel clk @ 27MHz */
+	 "CLAA-WVGA", 57, 800, 480, 37037, 40, 60, 10, 10, 20, 10,
 	 FB_SYNC_CLK_LAT_FALL,
 	 FB_VMODE_NONINTERLACED,
 	 0,},
@@ -77,13 +77,14 @@ static int lcd_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 {
 	struct fb_event *event = v;
 
-	if (strcmp(event->info->fix.id, "DISP3 BG")) {
+	if (strcmp(event->info->fix.id, "DISP3 BG") &&
+	    strcmp(event->info->fix.id, "mxc_elcdif_fb"))
 		return 0;
-	}
 
 	switch (val) {
 	case FB_EVENT_FB_REGISTERED:
 		lcd_init_fb(event->info);
+		fb_show_logo(event->info, 0);
 		lcd_poweron();
 		break;
 	case FB_EVENT_BLANK:
@@ -133,7 +134,8 @@ static int __devinit lcd_probe(struct platform_device *pdev)
 	}
 
 	for (i = 0; i < num_registered_fb; i++) {
-		if (strcmp(registered_fb[i]->fix.id, "DISP3 BG") == 0) {
+		if (strcmp(registered_fb[i]->fix.id, "DISP3 BG") == 0 ||
+		    strcmp(registered_fb[i]->fix.id, "mxc_elcdif_fb") == 0) {
 			lcd_init_fb(registered_fb[i]);
 			fb_show_logo(registered_fb[i], 0);
 			lcd_poweron();
diff --git a/drivers/video/mxc/tve.c b/drivers/video/mxc/tve.c
index 2d2929c0cbd9..793b470cdce4 100644
--- a/drivers/video/mxc/tve.c
+++ b/drivers/video/mxc/tve.c
@@ -68,6 +68,8 @@ static int enabled;		/* enable power on or not */
 DEFINE_SPINLOCK(tve_lock);
 
 static struct fb_info *tve_fbi;
+static struct fb_modelist tve_modelist;
+static bool g_enable_tve;
 
 struct tve_data {
 	struct platform_device *pdev;
@@ -77,6 +79,7 @@ struct tve_data {
 	int detect;
 	void *base;
 	int irq;
+	int blank;
 	struct clk *clk;
 	struct regulator *dac_reg;
 	struct regulator *dig_reg;
@@ -221,40 +224,50 @@ static int _is_tvout_mode_hd_compatible(void)
 static int tve_setup(int mode)
 {
 	u32 reg;
-	struct clk *pll3_clk;
-	unsigned long pll3_clock_rate = 216000000, di1_clock_rate = 27000000;
+	struct clk *tve_parent_clk;
+	unsigned long parent_clock_rate = 216000000, di1_clock_rate = 27000000;
+	unsigned long tve_clock_rate = 216000000;
 	struct clk *ipu_di1_clk;
 	unsigned long lock_flags;
 
-	if (tve.cur_mode == mode)
-		return 0;
-
 	spin_lock_irqsave(&tve_lock, lock_flags);
 
-	tve.cur_mode = mode;
-
 	switch (mode) {
 	case TVOUT_FMT_PAL:
 	case TVOUT_FMT_NTSC:
-		pll3_clock_rate = 216000000;
+		parent_clock_rate = 216000000;
 		di1_clock_rate = 27000000;
 		break;
 	case TVOUT_FMT_720P60:
-		pll3_clock_rate = 297000000;
+		parent_clock_rate = 297000000;
+		if (cpu_is_mx53())
+			tve_clock_rate = 297000000;
 		di1_clock_rate = 74250000;
 		break;
 	}
 	if (enabled)
 		clk_disable(tve.clk);
 
-	pll3_clk = clk_get(NULL, "pll3");
+	tve_parent_clk = clk_get_parent(tve.clk);
 	ipu_di1_clk = clk_get(NULL, "ipu_di1_clk");
 
-	clk_disable(pll3_clk);
-	clk_set_rate(pll3_clk, pll3_clock_rate);
-	clk_set_rate(ipu_di1_clk, di1_clock_rate);
+	clk_disable(tve_parent_clk);
+	clk_set_rate(tve_parent_clk, parent_clock_rate);
+
+	if (cpu_is_mx53())
+		clk_set_rate(tve.clk, tve_clock_rate);
 
 	clk_enable(tve.clk);
+	clk_set_rate(ipu_di1_clk, di1_clock_rate);
+
+	if (tve.cur_mode == mode) {
+		if (!enabled)
+			clk_disable(tve.clk);
+		spin_unlock_irqrestore(&tve_lock, lock_flags);
+		return 0;
+	}
+
+	tve.cur_mode = mode;
 
 	/* select output video format */
 	if (mode == TVOUT_FMT_PAL) {
@@ -497,6 +510,14 @@ static irqreturn_t tve_detect_handler(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+/* Re-construct clk for tve display */
+static inline void tve_recfg_fb(struct fb_info *fbi)
+{
+	fbi->flags &= ~FBINFO_MISC_USEREVENT;
+	fbi->var.activate |= FB_ACTIVATE_FORCE;
+	fb_set_var(fbi, &fbi->var);
+}
+
 int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 {
 	struct fb_event *event = v;
@@ -509,9 +530,9 @@ int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 			break;
 
 		tve_fbi = fbi;
-		fb_add_videomode(&video_modes[0], &tve_fbi->modelist);
-		fb_add_videomode(&video_modes[1], &tve_fbi->modelist);
-		fb_add_videomode(&video_modes[2], &tve_fbi->modelist);
+		fb_add_videomode(&video_modes[0], &tve_modelist.list);
+		fb_add_videomode(&video_modes[1], &tve_modelist.list);
+		fb_add_videomode(&video_modes[2], &tve_modelist.list);
 		break;
 	case FB_EVENT_MODE_CHANGE:
 	{
@@ -525,7 +546,7 @@ int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 
 		fb_var_to_videomode(&cur_mode, &fbi->var);
 
-		list_for_each(pos, &tve_fbi->modelist) {
+		list_for_each(pos, &tve_modelist.list) {
 			modelist = list_entry(pos, struct fb_modelist, list);
 			mode = &modelist->mode;
 			if (fb_mode_is_equal(&cur_mode, mode)) {
@@ -564,31 +585,33 @@ int tve_fb_event(struct notifier_block *nb, unsigned long val, void *v)
 			return 0;
 
 		if (*((int *)event->data) == FB_BLANK_UNBLANK) {
-			if (fb_mode_is_equal(fbi->mode, &video_modes[0])) {
-				if (tve.cur_mode != TVOUT_FMT_NTSC) {
+			if (tve.blank != FB_BLANK_UNBLANK) {
+				if (fb_mode_is_equal(fbi->mode, &video_modes[0])) {
 					tve_disable();
 					tve_setup(TVOUT_FMT_NTSC);
-				}
-				tve_enable();
-			} else if (fb_mode_is_equal(fbi->mode,
-					&video_modes[1])) {
-				if (tve.cur_mode != TVOUT_FMT_PAL) {
+					tve_enable();
+					tve_recfg_fb(fbi);
+				} else if (fb_mode_is_equal(fbi->mode,
+							&video_modes[1])) {
 					tve_disable();
 					tve_setup(TVOUT_FMT_PAL);
-				}
-				tve_enable();
-			} else if (fb_mode_is_equal(fbi->mode,
-					&video_modes[2])) {
-				if (tve.cur_mode != TVOUT_FMT_720P60) {
+					tve_enable();
+					tve_recfg_fb(fbi);
+				} else if (fb_mode_is_equal(fbi->mode,
+							&video_modes[2])) {
 					tve_disable();
 					tve_setup(TVOUT_FMT_720P60);
+					tve_enable();
+					tve_recfg_fb(fbi);
+				} else {
+					tve_setup(TVOUT_FMT_OFF);
 				}
-				tve_enable();
-			} else {
-				tve_setup(TVOUT_FMT_OFF);
+				tve.blank = FB_BLANK_UNBLANK;
 			}
-		} else
+		} else {
 			tve_disable();
+			tve.blank = FB_BLANK_POWERDOWN;
+		}
 		break;
 	}
 	return 0;
@@ -652,6 +675,11 @@ static int tve_probe(struct platform_device *pdev)
 	struct tve_platform_data *plat_data = pdev->dev.platform_data;
 	u32 conf_reg;
 
+	if (g_enable_tve == false)
+		return -ENODEV;
+
+	INIT_LIST_HEAD(&tve_modelist.list);
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (res == NULL)
 		return -ENOMEM;
@@ -699,9 +727,9 @@ static int tve_probe(struct platform_device *pdev)
 	}
 
 	if (tve_fbi != NULL) {
-		fb_add_videomode(&video_modes[0], &tve_fbi->modelist);
-		fb_add_videomode(&video_modes[1], &tve_fbi->modelist);
-		fb_add_videomode(&video_modes[2], &tve_fbi->modelist);
+		fb_add_videomode(&video_modes[0], &tve_modelist.list);
+		fb_add_videomode(&video_modes[1], &tve_modelist.list);
+		fb_add_videomode(&video_modes[2], &tve_modelist.list);
 	}
 
 	tve.dac_reg = regulator_get(&pdev->dev, plat_data->dac_reg);
@@ -748,22 +776,40 @@ static int tve_probe(struct platform_device *pdev)
 
 	clk_disable(tve.clk);
 
+	ret = fb_register_client(&nb);
+	if (ret < 0)
+		goto err2;
+
+	tve.blank = -1;
+
 	/* is primary display? */
 	if (primary) {
-		struct fb_event event;
+		struct fb_var_screeninfo var;
+		const struct fb_videomode *mode;
+
+		memset(&var, 0, sizeof(var));
+		mode = fb_match_mode(&tve_fbi->var, &tve_modelist.list);
+		if (mode) {
+			pr_debug("TVE: fb mode found\n");
+			fb_videomode_to_var(&var, mode);
+		} else {
+			pr_warning("TVE: can not find video mode\n");
+			goto done;
+		}
+		acquire_console_sem();
+		tve_fbi->flags |= FBINFO_MISC_USEREVENT;
+		fb_set_var(tve_fbi, &var);
+		tve_fbi->flags &= ~FBINFO_MISC_USEREVENT;
+		release_console_sem();
 
-		event.info = tve_fbi;
-		tve_fb_event(NULL, FB_EVENT_MODE_CHANGE, &event);
 		acquire_console_sem();
 		fb_blank(tve_fbi, FB_BLANK_UNBLANK);
 		release_console_sem();
+
 		fb_show_logo(tve_fbi, 0);
 	}
 
-	ret = fb_register_client(&nb);
-	if (ret < 0)
-		goto err2;
-
+done:
 	return 0;
 err2:
 	device_remove_file(&pdev->dev, &dev_attr_headphone);
@@ -842,6 +888,14 @@ static struct platform_driver tve_driver = {
 	.resume = tve_resume,
 };
 
+static int __init enable_tve_setup(char *options)
+{
+	g_enable_tve = true;
+
+	return 1;
+}
+__setup("tve", enable_tve_setup);
+
 static int __init tve_init(void)
 {
 	return platform_driver_register(&tve_driver);
diff --git a/drivers/video/mxs/Kconfig b/drivers/video/mxs/Kconfig
index aef4aa59dcad..35b896e95d4f 100644
--- a/drivers/video/mxs/Kconfig
+++ b/drivers/video/mxs/Kconfig
@@ -20,3 +20,9 @@ config FB_MXS_LCD_LMS430
 	default y if ARCH_MX23
 	---help---
 	  Use LMS430 dotclock LCD panel for MXS
+
+config FB_MXS_TVENC
+	depends on ARCH_MXS
+	bool "TVENC"
+	---help---
+	  Use TVOUT encoder for MXS
diff --git a/drivers/video/mxs/Makefile b/drivers/video/mxs/Makefile
index a9580add3757..fbab953718c7 100644
--- a/drivers/video/mxs/Makefile
+++ b/drivers/video/mxs/Makefile
@@ -2,3 +2,5 @@ obj-$(CONFIG_ARCH_MXS)				+= lcdif.o
 obj-$(CONFIG_FB_MXS)				+= mxsfb.o
 obj-$(CONFIG_FB_MXS_LCD_43WVF1G)		+= lcd_43wvf1g.o
 obj-$(CONFIG_FB_MXS_LCD_LMS430)			+= lcd_lms430.o
+# TVOUT support
+obj-$(CONFIG_FB_MXS_TVENC)			+= tvenc.o
diff --git a/drivers/watchdog/mxc_wdt.c b/drivers/watchdog/mxc_wdt.c
index 3626a51e557d..0114fab70253 100644
--- a/drivers/watchdog/mxc_wdt.c
+++ b/drivers/watchdog/mxc_wdt.c
@@ -200,7 +200,7 @@ mxc_wdt_ioctl(struct inode *inode, struct file *file,
 
 	switch (cmd) {
 	default:
-		return -ENOIOCTLCMD;
+		return -ENOTTY;
 	case WDIOC_GETSUPPORT:
 		return copy_to_user((struct watchdog_info __user *)arg, &ident,
 				    sizeof(ident));