Merge remote branch 'fsl-linux-sdk/imx_3.0.15' into imx_3.0.15_android

Conflicts:
	arch/arm/configs/imx6_defconfig
	arch/arm/kernel/smp.c
	arch/arm/mach-mx6/board-mx6q_arm2.c
	arch/arm/mach-mx6/board-mx6q_sabreauto.c
	arch/arm/mach-mx6/board-mx6q_sabrelite.c
	arch/arm/mach-mx6/board-mx6q_sabresd.c
	arch/arm/mach-mx6/bus_freq.c
	arch/arm/mach-mx6/clock.c
	arch/arm/mach-mx6/cpu.c
	arch/arm/mach-mx6/devices-imx6q.h
	arch/arm/mach-mx6/mx6_ddr_freq.S
	arch/arm/mach-mx6/mx6_mmdc.c
	arch/arm/mach-mx6/mx6_suspend.S
	arch/arm/mach-mx6/mx6_wfi.S
	arch/arm/mach-mx6/plat_hotplug.c
	arch/arm/mach-mx6/pm.c
	arch/arm/mach-mx6/system.c
	arch/arm/plat-mxc/cpufreq.c
	arch/arm/plat-mxc/system.c
	drivers/media/video/mxc/capture/Kconfig
	drivers/media/video/mxc/capture/mxc_v4l2_capture.c
	drivers/media/video/mxc/capture/ov5640_mipi.c
	drivers/media/video/mxc/output/mxc_vout.c
	drivers/mmc/host/sdhci.c
	drivers/mtd/nand/gpmi-nand/gpmi-lib.c
	drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_hardware.c
	drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu.c
	drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_enum.h
	drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_options.h
	drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_version.h
	drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c
	drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_os.c
	drivers/mxc/ipu3/vdoa.c
	drivers/power/max8903_charger.c
	drivers/video/mxc/mxc_ipuv3_fb.c
	sound/soc/codecs/mxc_spdif.c
	sound/soc/imx/Makefile
	sound/soc/imx/imx-wm8962.c

81 files changed, 9378 insertions, 1033 deletions

diff --git a/drivers/cpufreq/cpufreq_interactive.c b/drivers/cpufreq/cpufreq_interactive.c
index 123c86409ec1..2fc2f5f49e54 100644
--- a/drivers/cpufreq/cpufreq_interactive.c
+++ b/drivers/cpufreq/cpufreq_interactive.c
@@ -827,7 +827,7 @@ int cpufreq_gov_irq_tuner_register(struct irq_tuner dbs_irq_tuner)
 }
 EXPORT_SYMBOL_GPL(cpufreq_gov_irq_tuner_register);
 #ifdef CONFIG_CPU_FREQ_DEFAULT_GOV_INTERACTIVE
-fs_initcall(cpufreq_interactive_init);
+late_initcall(cpufreq_interactive_init);
 #else
 module_init(cpufreq_interactive_init);
 #endif
diff --git a/drivers/crypto/caam/Kconfig b/drivers/crypto/caam/Kconfig
index 2d876bb98ff4..82a1011978d8 100644
--- a/drivers/crypto/caam/Kconfig
+++ b/drivers/crypto/caam/Kconfig
@@ -1,6 +1,6 @@
 config CRYPTO_DEV_FSL_CAAM
 	tristate "Freescale CAAM-Multicore driver backend"
-	depends on FSL_SOC
+	depends on FSL_SOC || ARCH_MXC
 	help
 	  Enables the driver module for Freescale's Cryptographic Accelerator
 	  and Assurance Module (CAAM), also known as the SEC version 4 (SEC4).
diff --git a/drivers/crypto/caam/caamalg.c b/drivers/crypto/caam/caamalg.c
index 676d957c22b0..3c3e76b6ee1f 100644
--- a/drivers/crypto/caam/caamalg.c
+++ b/drivers/crypto/caam/caamalg.c
@@ -1,7 +1,7 @@
 /*
  * caam - Freescale FSL CAAM support for crypto API
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2008-2012 Freescale Semiconductor, Inc.
  *
  * Based on talitos crypto API driver.
  *
@@ -62,10 +62,22 @@
 #define CAAM_MAX_IV_LENGTH		16
 
 /* length of descriptors text */
-#define DESC_AEAD_SHARED_TEXT_LEN	4
-#define DESC_AEAD_ENCRYPT_TEXT_LEN 	21
-#define DESC_AEAD_DECRYPT_TEXT_LEN 	24
-#define DESC_AEAD_GIVENCRYPT_TEXT_LEN 	27
+#define DESC_JOB_IO_LEN			(CAAM_CMD_SZ * 3 + CAAM_PTR_SZ * 3)
+
+#define DESC_AEAD_BASE			(4 * CAAM_CMD_SZ)
+#define DESC_AEAD_ENC_LEN		(DESC_AEAD_BASE + 16 * CAAM_CMD_SZ)
+#define DESC_AEAD_DEC_LEN		(DESC_AEAD_BASE + 21 * CAAM_CMD_SZ)
+#define DESC_AEAD_GIVENC_LEN		(DESC_AEAD_ENC_LEN + 7 * CAAM_CMD_SZ)
+
+#define DESC_ABLKCIPHER_BASE		(3 * CAAM_CMD_SZ)
+#define DESC_ABLKCIPHER_ENC_LEN		(DESC_ABLKCIPHER_BASE + \
+					 20 * CAAM_CMD_SZ)
+#define DESC_ABLKCIPHER_DEC_LEN		(DESC_ABLKCIPHER_BASE + \
+					 15 * CAAM_CMD_SZ)
+
+#define DESC_MAX_USED_BYTES		(DESC_AEAD_GIVENC_LEN + \
+					 CAAM_MAX_KEY_SIZE)
+#define DESC_MAX_USED_LEN		(DESC_MAX_USED_BYTES / CAAM_CMD_SZ)
 
 #ifdef DEBUG
 /* for print_hex_dumps with line references */
@@ -76,30 +88,372 @@
 #define debug(format, arg...)
 #endif
 
+/* Set DK bit in class 1 operation if shared */
+static inline void append_dec_op1(u32 *desc, u32 type)
+{
+	u32 *jump_cmd, *uncond_jump_cmd;
+
+	jump_cmd = append_jump(desc, JUMP_TEST_ALL | JUMP_COND_SHRD);
+	append_operation(desc, type | OP_ALG_AS_INITFINAL |
+			 OP_ALG_DECRYPT);
+	uncond_jump_cmd = append_jump(desc, JUMP_TEST_ALL);
+	set_jump_tgt_here(desc, jump_cmd);
+	append_operation(desc, type | OP_ALG_AS_INITFINAL |
+			 OP_ALG_DECRYPT | OP_ALG_AAI_DK);
+	set_jump_tgt_here(desc, uncond_jump_cmd);
+}
+
+/*
+ * Wait for completion of class 1 key loading before allowing
+ * error propagation
+ */
+static inline void append_dec_shr_done(u32 *desc)
+{
+	u32 *jump_cmd;
+
+	jump_cmd = append_jump(desc, JUMP_CLASS_CLASS1 | JUMP_TEST_ALL);
+	set_jump_tgt_here(desc, jump_cmd);
+	append_cmd(desc, SET_OK_PROP_ERRORS | CMD_LOAD);
+}
+
+/*
+ * For aead functions, read payload and write payload,
+ * both of which are specified in req->src and req->dst
+ */
+static inline void aead_append_src_dst(u32 *desc, u32 msg_type)
+{
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_BOTH |
+			     KEY_VLF | msg_type | FIFOLD_TYPE_LASTBOTH);
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | KEY_VLF);
+}
+
+/*
+ * For aead encrypt and decrypt, read iv for both classes
+ */
+static inline void aead_append_ld_iv(u32 *desc, int ivsize)
+{
+	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_BYTE_CONTEXT |
+		   LDST_CLASS_1_CCB | ivsize);
+	append_move(desc, MOVE_SRC_CLASS1CTX | MOVE_DEST_CLASS2INFIFO | ivsize);
+}
+
+/*
+ * For ablkcipher encrypt and decrypt, read from req->src and
+ * write to req->dst
+ */
+static inline void ablkcipher_append_src_dst(u32 *desc)
+{
+	append_math_add(desc, VARSEQOUTLEN, SEQINLEN, REG0, CAAM_CMD_SZ); \
+	append_math_add(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ); \
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS1 | \
+			     KEY_VLF | FIFOLD_TYPE_MSG | FIFOLD_TYPE_LAST1); \
+	append_seq_fifo_store(desc, 0, FIFOST_TYPE_MESSAGE_DATA | KEY_VLF); \
+}
+
+/*
+ * If all data, including src (with assoc and iv) or dst (with iv only) are
+ * contiguous
+ */
+#define GIV_SRC_CONTIG		1
+#define GIV_DST_CONTIG		(1 << 1)
+
 /*
  * per-session context
  */
 struct caam_ctx {
 	struct device *jrdev;
-	u32 *sh_desc;
-	dma_addr_t shared_desc_phys;
+	u32 sh_desc_enc[DESC_MAX_USED_LEN];
+	u32 sh_desc_dec[DESC_MAX_USED_LEN];
+	u32 sh_desc_givenc[DESC_MAX_USED_LEN];
+	dma_addr_t sh_desc_enc_dma;
+	dma_addr_t sh_desc_dec_dma;
+	dma_addr_t sh_desc_givenc_dma;
 	u32 class1_alg_type;
 	u32 class2_alg_type;
 	u32 alg_op;
-	u8 *key;
-	dma_addr_t key_phys;
+	u8 key[CAAM_MAX_KEY_SIZE];
+	dma_addr_t key_dma;
 	unsigned int enckeylen;
 	unsigned int split_key_len;
 	unsigned int split_key_pad_len;
 	unsigned int authsize;
 };
 
-static int aead_authenc_setauthsize(struct crypto_aead *authenc,
+static void append_key_aead(u32 *desc, struct caam_ctx *ctx,
+			    int keys_fit_inline)
+{
+	if (keys_fit_inline) {
+		append_key_as_imm(desc, ctx->key, ctx->split_key_pad_len,
+				  ctx->split_key_len, CLASS_2 |
+				  KEY_DEST_MDHA_SPLIT | KEY_ENC);
+		append_key_as_imm(desc, (void *)ctx->key +
+				  ctx->split_key_pad_len, ctx->enckeylen,
+				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	} else {
+		append_key(desc, ctx->key_dma, ctx->split_key_len, CLASS_2 |
+			   KEY_DEST_MDHA_SPLIT | KEY_ENC);
+		append_key(desc, ctx->key_dma + ctx->split_key_pad_len,
+			   ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
+	}
+}
+
+static void init_sh_desc_key_aead(u32 *desc, struct caam_ctx *ctx,
+				  int keys_fit_inline)
+{
+	u32 *key_jump_cmd;
+
+	init_sh_desc(desc, HDR_SHARE_WAIT);
+
+	/* Skip if already shared */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD);
+
+	append_key_aead(desc, ctx, keys_fit_inline);
+
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* Propagate errors from shared to job descriptor */
+	append_cmd(desc, SET_OK_PROP_ERRORS | CMD_LOAD);
+}
+
+static int aead_set_sh_desc(struct crypto_aead *aead)
+{
+	struct aead_tfm *tfm = &aead->base.crt_aead;
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	bool keys_fit_inline = 0;
+	u32 *key_jump_cmd, *jump_cmd;
+	u32 geniv, moveiv;
+	u32 *desc;
+
+	if (!ctx->enckeylen || !ctx->authsize)
+		return 0;
+
+	/*
+	 * Job Descriptor and Shared Descriptors
+	 * must all fit into the 64-word Descriptor h/w Buffer
+	 */
+	if (DESC_AEAD_ENC_LEN + DESC_JOB_IO_LEN +
+	    ctx->split_key_pad_len + ctx->enckeylen <=
+	    CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = 1;
+
+	/* aead_encrypt shared descriptor */
+	desc = ctx->sh_desc_enc;
+
+	init_sh_desc_key_aead(desc, ctx, keys_fit_inline);
+
+	/* Class 2 operation */
+	append_operation(desc, ctx->class2_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* cryptlen = seqoutlen - authsize */
+	append_math_sub_imm_u32(desc, REG3, SEQOUTLEN, IMM, ctx->authsize);
+
+	/* assoclen + cryptlen = seqinlen - ivsize */
+	append_math_sub_imm_u32(desc, REG2, SEQINLEN, IMM, tfm->ivsize);
+
+	/* assoclen + cryptlen = (assoclen + cryptlen) - cryptlen */
+	append_math_sub(desc, VARSEQINLEN, REG2, REG3, CAAM_CMD_SZ);
+
+	/* read assoc before reading payload */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS2 | FIFOLD_TYPE_MSG |
+			     KEY_VLF);
+	aead_append_ld_iv(desc, tfm->ivsize);
+
+	/* Class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* Read and write cryptlen bytes */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG3, CAAM_CMD_SZ);
+	append_math_add(desc, VARSEQOUTLEN, ZERO, REG3, CAAM_CMD_SZ);
+	aead_append_src_dst(desc, FIFOLD_TYPE_MSG1OUT2);
+
+	/* Write ICV */
+	append_seq_store(desc, ctx->authsize, LDST_CLASS_2_CCB |
+			 LDST_SRCDST_BYTE_CONTEXT);
+
+	ctx->sh_desc_enc_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_enc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "aead enc shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+
+	dma_sync_single_for_cpu(jrdev, ctx->sh_desc_enc_dma, desc_bytes(desc),
+				DMA_TO_DEVICE);
+	/*
+	 * Job Descriptor and Shared Descriptors
+	 * must all fit into the 64-word Descriptor h/w Buffer
+	 */
+	if (DESC_AEAD_DEC_LEN + DESC_JOB_IO_LEN +
+	    ctx->split_key_pad_len + ctx->enckeylen <=
+	    CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = 1;
+
+	desc = ctx->sh_desc_dec;
+
+	/* aead_decrypt shared descriptor */
+	init_sh_desc(desc, HDR_SHARE_WAIT);
+
+	/* Skip if already shared */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD);
+
+	append_key_aead(desc, ctx, keys_fit_inline);
+
+	/* Only propagate error immediately if shared */
+	jump_cmd = append_jump(desc, JUMP_TEST_ALL);
+	set_jump_tgt_here(desc, key_jump_cmd);
+	append_cmd(desc, SET_OK_PROP_ERRORS | CMD_LOAD);
+	set_jump_tgt_here(desc, jump_cmd);
+
+	/* Class 2 operation */
+	append_operation(desc, ctx->class2_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_DECRYPT | OP_ALG_ICV_ON);
+
+	/* assoclen + cryptlen = seqinlen - ivsize */
+	append_math_sub_imm_u32(desc, REG3, SEQINLEN, IMM,
+				ctx->authsize + tfm->ivsize)
+	/* assoclen = (assoclen + cryptlen) - cryptlen */
+	append_math_sub(desc, REG2, SEQOUTLEN, REG0, CAAM_CMD_SZ);
+	append_math_sub(desc, VARSEQINLEN, REG3, REG2, CAAM_CMD_SZ);
+
+	/* read assoc before reading payload */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS2 | FIFOLD_TYPE_MSG |
+			     KEY_VLF);
+
+	aead_append_ld_iv(desc, tfm->ivsize);
+
+	append_dec_op1(desc, ctx->class1_alg_type);
+
+	/* Read and write cryptlen bytes */
+	append_math_add(desc, VARSEQINLEN, ZERO, REG2, CAAM_CMD_SZ);
+	append_math_add(desc, VARSEQOUTLEN, ZERO, REG2, CAAM_CMD_SZ);
+	aead_append_src_dst(desc, FIFOLD_TYPE_MSG);
+
+	/* Load ICV */
+	append_seq_fifo_load(desc, ctx->authsize, FIFOLD_CLASS_CLASS2 |
+			     FIFOLD_TYPE_LAST2 | FIFOLD_TYPE_ICV);
+	append_dec_shr_done(desc);
+
+	ctx->sh_desc_dec_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_dec_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "aead dec shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+	dma_sync_single_for_cpu(jrdev, ctx->sh_desc_dec_dma, desc_bytes(desc),
+				DMA_TO_DEVICE);
+
+	/*
+	 * Job Descriptor and Shared Descriptors
+	 * must all fit into the 64-word Descriptor h/w Buffer
+	 */
+	if (DESC_AEAD_GIVENC_LEN + DESC_JOB_IO_LEN +
+	    ctx->split_key_pad_len + ctx->enckeylen <=
+	    CAAM_DESC_BYTES_MAX)
+		keys_fit_inline = 1;
+
+	/* aead_givencrypt shared descriptor */
+	desc = ctx->sh_desc_givenc;
+
+	init_sh_desc_key_aead(desc, ctx, keys_fit_inline);
+
+	/* Generate IV */
+	geniv = NFIFOENTRY_STYPE_PAD | NFIFOENTRY_DEST_DECO |
+		NFIFOENTRY_DTYPE_MSG | NFIFOENTRY_LC1 |
+		NFIFOENTRY_PTYPE_RND | (tfm->ivsize << NFIFOENTRY_DLEN_SHIFT);
+	append_load_imm_u32(desc, geniv, LDST_CLASS_IND_CCB |
+			    LDST_SRCDST_WORD_INFO_FIFO | LDST_IMM);
+	append_cmd(desc, CMD_LOAD | DISABLE_AUTO_INFO_FIFO);
+	append_move(desc, MOVE_SRC_INFIFO |
+		    MOVE_DEST_CLASS1CTX | (tfm->ivsize << MOVE_LEN_SHIFT));
+	append_cmd(desc, CMD_LOAD | ENABLE_AUTO_INFO_FIFO);
+
+	/* Copy IV to class 1 context */
+	append_move(desc, MOVE_SRC_CLASS1CTX |
+		    MOVE_DEST_OUTFIFO | (tfm->ivsize << MOVE_LEN_SHIFT));
+
+	/* Return to encryption */
+	append_operation(desc, ctx->class2_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* ivsize + cryptlen = seqoutlen - authsize */
+	append_math_sub_imm_u32(desc, REG3, SEQOUTLEN, IMM, ctx->authsize);
+
+	/* assoclen = seqinlen - (ivsize + cryptlen) */
+	append_math_sub(desc, VARSEQINLEN, SEQINLEN, REG3, CAAM_CMD_SZ);
+
+	/* read assoc before reading payload */
+	append_seq_fifo_load(desc, 0, FIFOLD_CLASS_CLASS2 | FIFOLD_TYPE_MSG |
+			     KEY_VLF);
+
+	/* Copy iv from class 1 ctx to class 2 fifo*/
+	moveiv = NFIFOENTRY_STYPE_OFIFO | NFIFOENTRY_DEST_CLASS2 |
+		 NFIFOENTRY_DTYPE_MSG | (tfm->ivsize << NFIFOENTRY_DLEN_SHIFT);
+	append_load_imm_u32(desc, moveiv, LDST_CLASS_IND_CCB |
+			    LDST_SRCDST_WORD_INFO_FIFO | LDST_IMM);
+	append_load_imm_u32(desc, tfm->ivsize, LDST_CLASS_2_CCB |
+			    LDST_SRCDST_WORD_DATASZ_REG | LDST_IMM);
+
+	/* Class 1 operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* Will write ivsize + cryptlen */
+	append_math_add(desc, VARSEQOUTLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+
+	/* Not need to reload iv */
+	append_seq_fifo_load(desc, tfm->ivsize,
+			     FIFOLD_CLASS_SKIP);
+
+	/* Will read cryptlen */
+	append_math_add(desc, VARSEQINLEN, SEQINLEN, REG0, CAAM_CMD_SZ);
+	aead_append_src_dst(desc, FIFOLD_TYPE_MSG1OUT2);
+
+	/* Write ICV */
+	append_seq_store(desc, ctx->authsize, LDST_CLASS_2_CCB |
+			 LDST_SRCDST_BYTE_CONTEXT);
+
+	ctx->sh_desc_givenc_dma = dma_map_single(jrdev, desc,
+						 desc_bytes(desc),
+						 DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_givenc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "aead givenc shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+	dma_sync_single_for_cpu(jrdev, ctx->sh_desc_givenc_dma,
+				desc_bytes(desc), DMA_TO_DEVICE);
+
+	return 0;
+}
+
+static int aead_setauthsize(struct crypto_aead *authenc,
 				    unsigned int authsize)
 {
 	struct caam_ctx *ctx = crypto_aead_ctx(authenc);
 
 	ctx->authsize = authsize;
+	aead_set_sh_desc(authenc);
 
 	return 0;
 }
@@ -117,6 +471,7 @@ static void split_key_done(struct device *dev, u32 *desc, u32 err,
 #ifdef DEBUG
 	dev_err(dev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
 #endif
+
 	if (err) {
 		char tmp[CAAM_ERROR_STR_MAX];
 
@@ -161,6 +516,9 @@ static u32 gen_split_key(struct caam_ctx *ctx, const u8 *key_in, u32 authkeylen)
 		kfree(desc);
 		return -ENOMEM;
 	}
+	dma_sync_single_for_device(jrdev, dma_addr_in, authkeylen,
+				   DMA_TO_DEVICE);
+
 	append_key(desc, dma_addr_in, authkeylen, CLASS_2 |
 		       KEY_DEST_CLASS_REG);
 
@@ -211,6 +569,8 @@ static u32 gen_split_key(struct caam_ctx *ctx, const u8 *key_in, u32 authkeylen)
 #endif
 	}
 
+	dma_sync_single_for_cpu(jrdev, dma_addr_out, ctx->split_key_pad_len,
+				DMA_FROM_DEVICE);
 	dma_unmap_single(jrdev, dma_addr_out, ctx->split_key_pad_len,
 			 DMA_FROM_DEVICE);
 	dma_unmap_single(jrdev, dma_addr_in, authkeylen, DMA_TO_DEVICE);
@@ -220,73 +580,7 @@ static u32 gen_split_key(struct caam_ctx *ctx, const u8 *key_in, u32 authkeylen)
 	return ret;
 }
 
-static int build_sh_desc_ipsec(struct caam_ctx *ctx)
-{
-	struct device *jrdev = ctx->jrdev;
-	u32 *sh_desc;
-	u32 *jump_cmd;
-	bool keys_fit_inline = 0;
-
-	/*
-	 * largest Job Descriptor and its Shared Descriptor
-	 * must both fit into the 64-word Descriptor h/w Buffer
-	 */
-	if ((DESC_AEAD_GIVENCRYPT_TEXT_LEN +
-	     DESC_AEAD_SHARED_TEXT_LEN) * CAAM_CMD_SZ +
-	    ctx->split_key_pad_len + ctx->enckeylen <= CAAM_DESC_BYTES_MAX)
-		keys_fit_inline = 1;
-
-	/* build shared descriptor for this session */
-	sh_desc = kmalloc(CAAM_CMD_SZ * DESC_AEAD_SHARED_TEXT_LEN +
-			  (keys_fit_inline ?
-			   ctx->split_key_pad_len + ctx->enckeylen :
-			   CAAM_PTR_SZ * 2), GFP_DMA | GFP_KERNEL);
-	if (!sh_desc) {
-		dev_err(jrdev, "could not allocate shared descriptor\n");
-		return -ENOMEM;
-	}
-
-	init_sh_desc(sh_desc, HDR_SAVECTX | HDR_SHARE_SERIAL);
-
-	jump_cmd = append_jump(sh_desc, CLASS_BOTH | JUMP_TEST_ALL |
-			       JUMP_COND_SHRD | JUMP_COND_SELF);
-
-	/*
-	 * process keys, starting with class 2/authentication.
-	 */
-	if (keys_fit_inline) {
-		append_key_as_imm(sh_desc, ctx->key, ctx->split_key_pad_len,
-				  ctx->split_key_len,
-				  CLASS_2 | KEY_DEST_MDHA_SPLIT | KEY_ENC);
-
-		append_key_as_imm(sh_desc, (void *)ctx->key +
-				  ctx->split_key_pad_len, ctx->enckeylen,
-				  ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
-	} else {
-		append_key(sh_desc, ctx->key_phys, ctx->split_key_len, CLASS_2 |
-			   KEY_DEST_MDHA_SPLIT | KEY_ENC);
-		append_key(sh_desc, ctx->key_phys + ctx->split_key_pad_len,
-			   ctx->enckeylen, CLASS_1 | KEY_DEST_CLASS_REG);
-	}
-
-	/* update jump cmd now that we are at the jump target */
-	set_jump_tgt_here(sh_desc, jump_cmd);
-
-	ctx->shared_desc_phys = dma_map_single(jrdev, sh_desc,
-					       desc_bytes(sh_desc),
-					       DMA_TO_DEVICE);
-	if (dma_mapping_error(jrdev, ctx->shared_desc_phys)) {
-		dev_err(jrdev, "unable to map shared descriptor\n");
-		kfree(sh_desc);
-		return -ENOMEM;
-	}
-
-	ctx->sh_desc = sh_desc;
-
-	return 0;
-}
-
-static int aead_authenc_setkey(struct crypto_aead *aead,
+static int aead_setkey(struct crypto_aead *aead,
 			       const u8 *key, unsigned int keylen)
 {
 	/* Sizes for MDHA pads (*not* keys): MD5, SHA1, 224, 256, 384, 512 */
@@ -326,27 +620,19 @@ static int aead_authenc_setkey(struct crypto_aead *aead,
 	print_hex_dump(KERN_ERR, "key in @"xstr(__LINE__)": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4, key, keylen, 1);
 #endif
-	ctx->key = kmalloc(ctx->split_key_pad_len + enckeylen,
-			   GFP_KERNEL | GFP_DMA);
-	if (!ctx->key) {
-		dev_err(jrdev, "could not allocate key output memory\n");
-		return -ENOMEM;
-	}
 
 	ret = gen_split_key(ctx, key, authkeylen);
 	if (ret) {
-		kfree(ctx->key);
 		goto badkey;
 	}
 
 	/* postpend encryption key to auth split key */
 	memcpy(ctx->key + ctx->split_key_pad_len, key + authkeylen, enckeylen);
 
-	ctx->key_phys = dma_map_single(jrdev, ctx->key, ctx->split_key_pad_len +
+	ctx->key_dma = dma_map_single(jrdev, ctx->key, ctx->split_key_pad_len +
 				       enckeylen, DMA_TO_DEVICE);
-	if (dma_mapping_error(jrdev, ctx->key_phys)) {
+	if (dma_mapping_error(jrdev, ctx->key_dma)) {
 		dev_err(jrdev, "unable to map key i/o memory\n");
-		kfree(ctx->key);
 		return -ENOMEM;
 	}
 #ifdef DEBUG
@@ -354,14 +640,16 @@ static int aead_authenc_setkey(struct crypto_aead *aead,
 		       DUMP_PREFIX_ADDRESS, 16, 4, ctx->key,
 		       ctx->split_key_pad_len + enckeylen, 1);
 #endif
+	dma_sync_single_for_device(jrdev, ctx->key_dma,
+				   ctx->split_key_pad_len + enckeylen,
+				   DMA_TO_DEVICE);
 
 	ctx->enckeylen = enckeylen;
 
-	ret = build_sh_desc_ipsec(ctx);
+	ret = aead_set_sh_desc(aead);
 	if (ret) {
-		dma_unmap_single(jrdev, ctx->key_phys, ctx->split_key_pad_len +
+		dma_unmap_single(jrdev, ctx->key_dma, ctx->split_key_pad_len +
 				 enckeylen, DMA_TO_DEVICE);
-		kfree(ctx->key);
 	}
 
 	return ret;
@@ -370,73 +658,257 @@ badkey:
 	return -EINVAL;
 }
 
+static int ablkcipher_setkey(struct crypto_ablkcipher *ablkcipher,
+			     const u8 *key, unsigned int keylen)
+{
+	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
+	struct ablkcipher_tfm *tfm = &ablkcipher->base.crt_ablkcipher;
+	struct device *jrdev = ctx->jrdev;
+	int ret = 0;
+	u32 *key_jump_cmd, *jump_cmd;
+	u32 *desc;
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "key in @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, key, keylen, 1);
+#endif
+
+	memcpy(ctx->key, key, keylen);
+	ctx->key_dma = dma_map_single(jrdev, ctx->key, keylen,
+				      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->key_dma)) {
+		dev_err(jrdev, "unable to map key i/o memory\n");
+		return -ENOMEM;
+	}
+	ctx->enckeylen = keylen;
+	dma_sync_single_for_device(jrdev, ctx->key_dma, keylen, DMA_TO_DEVICE);
+
+	/* ablkcipher_encrypt shared descriptor */
+	desc = ctx->sh_desc_enc;
+	init_sh_desc(desc, HDR_SHARE_WAIT);
+	/* Skip if already shared */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD);
+
+	/* Load class1 key only */
+	append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+			  ctx->enckeylen, CLASS_1 |
+			  KEY_DEST_CLASS_REG);
+
+	set_jump_tgt_here(desc, key_jump_cmd);
+
+	/* Propagate errors from shared to job descriptor */
+	append_cmd(desc, SET_OK_PROP_ERRORS | CMD_LOAD);
+
+	/* Load iv */
+	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_BYTE_CONTEXT |
+		   LDST_CLASS_1_CCB | tfm->ivsize);
+
+	/* Load operation */
+	append_operation(desc, ctx->class1_alg_type |
+			 OP_ALG_AS_INITFINAL | OP_ALG_ENCRYPT);
+
+	/* Perform operation */
+	ablkcipher_append_src_dst(desc);
+
+	ctx->sh_desc_enc_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_enc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ablkcipher enc shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+	dma_sync_single_for_device(jrdev, ctx->sh_desc_enc_dma,
+				   desc_bytes(desc), DMA_TO_DEVICE);
+
+	/* ablkcipher_decrypt shared descriptor */
+	desc = ctx->sh_desc_dec;
+
+	init_sh_desc(desc, HDR_SHARE_WAIT);
+	/* Skip if already shared */
+	key_jump_cmd = append_jump(desc, JUMP_JSL | JUMP_TEST_ALL |
+				   JUMP_COND_SHRD);
+
+	/* Load class1 key only */
+	append_key_as_imm(desc, (void *)ctx->key, ctx->enckeylen,
+			  ctx->enckeylen, CLASS_1 |
+			  KEY_DEST_CLASS_REG);
+
+	/* For aead, only propagate error immediately if shared */
+	jump_cmd = append_jump(desc, JUMP_TEST_ALL);
+	set_jump_tgt_here(desc, key_jump_cmd);
+	append_cmd(desc, SET_OK_PROP_ERRORS | CMD_LOAD);
+	set_jump_tgt_here(desc, jump_cmd);
+
+	/* load IV */
+	append_cmd(desc, CMD_SEQ_LOAD | LDST_SRCDST_BYTE_CONTEXT |
+		   LDST_CLASS_1_CCB | tfm->ivsize);
+
+	/* Choose operation */
+	append_dec_op1(desc, ctx->class1_alg_type);
+
+	/* Perform operation */
+	ablkcipher_append_src_dst(desc);
+
+	/* Wait for key to load before allowing propagating error */
+	append_dec_shr_done(desc);
+
+	ctx->sh_desc_dec_dma = dma_map_single(jrdev, desc,
+					      desc_bytes(desc),
+					      DMA_TO_DEVICE);
+	if (dma_mapping_error(jrdev, ctx->sh_desc_enc_dma)) {
+		dev_err(jrdev, "unable to map shared descriptor\n");
+		return -ENOMEM;
+	}
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ablkcipher dec shdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, desc,
+		       desc_bytes(desc), 1);
+#endif
+	dma_sync_single_for_device(jrdev, ctx->sh_desc_dec_dma,
+				   desc_bytes(desc), DMA_TO_DEVICE);
+
+	return ret;
+}
+
+/*
+ * 16-byte hardware scatter/gather table
+ * An 8-byte table exists in the hardware spec, but has never been
+ * implemented to date. The 8/16 option is selected at RTL-compile-time.
+ * and this selection is visible in the Compile Time Parameters Register
+ */
+
+#define LINKTBL_EXT		0x80000000	/* Entry points to table */
+#define LINKTBL_FINAL		0x40000000	/* Last ent in table */
+#define LINKTBL_BPID_MASK	0x000000ff
+#define LINKTBL_BPID_SHIFT	16
+#define LINKTBL_LEN_MASK	0x3fffffff	/* Excludes EXT and FINAL */
+#define LINKTBL_OFFS_MASK	0x00001fff
+
 struct link_tbl_entry {
+#ifdef CONFIG_64BIT
 	u64 ptr;
+#else
+	u32 reserved;
+	u32 ptr;
+#endif
 	u32 len;
-	u8 reserved;
-	u8 buf_pool_id;
-	u16 offset;
+	u32 bpid_offset; /* BPID in high, offset in lowest bits */
 };
 
 /*
- * ipsec_esp_edesc - s/w-extended ipsec_esp descriptor
+ * aead_edesc - s/w-extended aead descriptor
+ * @assoc_nents: number of segments in associated data (SPI+Seq) scatterlist
  * @src_nents: number of segments in input scatterlist
  * @dst_nents: number of segments in output scatterlist
- * @assoc_nents: number of segments in associated data (SPI+Seq) scatterlist
+ * @iv_dma: dma address of iv for checking continuity and link table
  * @desc: h/w descriptor (variable length; must not exceed MAX_CAAM_DESCSIZE)
  * @link_tbl_bytes: length of dma mapped link_tbl space
  * @link_tbl_dma: bus physical mapped address of h/w link table
  * @hw_desc: the h/w job descriptor followed by any referenced link tables
  */
-struct ipsec_esp_edesc {
+struct aead_edesc {
 	int assoc_nents;
 	int src_nents;
 	int dst_nents;
+	dma_addr_t iv_dma;
 	int link_tbl_bytes;
 	dma_addr_t link_tbl_dma;
 	struct link_tbl_entry *link_tbl;
 	u32 hw_desc[0];
 };
 
-static void ipsec_esp_unmap(struct device *dev,
-			    struct ipsec_esp_edesc *edesc,
-			    struct aead_request *areq)
-{
-	dma_unmap_sg(dev, areq->assoc, edesc->assoc_nents, DMA_TO_DEVICE);
+/*
+ * ablkcipher_edesc - s/w-extended ablkcipher descriptor
+ * @src_nents: number of segments in input scatterlist
+ * @dst_nents: number of segments in output scatterlist
+ * @iv_dma: dma address of iv for checking continuity and link table
+ * @desc: h/w descriptor (variable length; must not exceed MAX_CAAM_DESCSIZE)
+ * @link_tbl_bytes: length of dma mapped link_tbl space
+ * @link_tbl_dma: bus physical mapped address of h/w link table
+ * @hw_desc: the h/w job descriptor followed by any referenced link tables
+ */
+struct ablkcipher_edesc {
+	int src_nents;
+	int dst_nents;
+	dma_addr_t iv_dma;
+	int link_tbl_bytes;
+	dma_addr_t link_tbl_dma;
+	struct link_tbl_entry *link_tbl;
+	u32 hw_desc[0];
+};
 
-	if (unlikely(areq->dst != areq->src)) {
-		dma_unmap_sg(dev, areq->src, edesc->src_nents,
-			     DMA_TO_DEVICE);
-		dma_unmap_sg(dev, areq->dst, edesc->dst_nents,
-			     DMA_FROM_DEVICE);
+static void caam_unmap(struct device *dev, struct scatterlist *src,
+		       struct scatterlist *dst, int src_nents, int dst_nents,
+		       dma_addr_t iv_dma, int ivsize, dma_addr_t link_tbl_dma,
+		       int link_tbl_bytes)
+{
+	if (unlikely(dst != src)) {
+		dma_unmap_sg(dev, src, src_nents, DMA_TO_DEVICE);
+		dma_sync_sg_for_cpu(dev, dst, dst_nents, DMA_FROM_DEVICE);
+		dma_unmap_sg(dev, dst, dst_nents, DMA_FROM_DEVICE);
 	} else {
-		dma_unmap_sg(dev, areq->src, edesc->src_nents,
-			     DMA_BIDIRECTIONAL);
+		dma_sync_sg_for_cpu(dev, src, src_nents, DMA_BIDIRECTIONAL);
+		dma_unmap_sg(dev, src, src_nents, DMA_BIDIRECTIONAL);
 	}
 
-	if (edesc->link_tbl_bytes)
-		dma_unmap_single(dev, edesc->link_tbl_dma,
-				 edesc->link_tbl_bytes,
+	if (iv_dma)
+		dma_unmap_single(dev, iv_dma, ivsize, DMA_TO_DEVICE);
+
+	if (link_tbl_bytes)
+		dma_unmap_single(dev, link_tbl_dma, link_tbl_bytes,
 				 DMA_TO_DEVICE);
 }
 
-/*
- * ipsec_esp descriptor callbacks
- */
-static void ipsec_esp_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
+static void aead_unmap(struct device *dev,
+		       struct aead_edesc *edesc,
+		       struct aead_request *req)
+{
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	int ivsize = crypto_aead_ivsize(aead);
+
+	dma_unmap_sg(dev, req->assoc, edesc->assoc_nents, DMA_TO_DEVICE);
+
+	caam_unmap(dev, req->src, req->dst,
+		   edesc->src_nents, edesc->dst_nents,
+		   edesc->iv_dma, ivsize, edesc->link_tbl_dma,
+		   edesc->link_tbl_bytes);
+}
+
+static void ablkcipher_unmap(struct device *dev,
+			     struct ablkcipher_edesc *edesc,
+			     struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	int ivsize = crypto_ablkcipher_ivsize(ablkcipher);
+
+	caam_unmap(dev, req->src, req->dst,
+		   edesc->src_nents, edesc->dst_nents,
+		   edesc->iv_dma, ivsize, edesc->link_tbl_dma,
+		   edesc->link_tbl_bytes);
+}
+
+static void aead_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
 				   void *context)
 {
-	struct aead_request *areq = context;
-	struct ipsec_esp_edesc *edesc;
+	struct aead_request *req = context;
+	struct aead_edesc *edesc;
 #ifdef DEBUG
-	struct crypto_aead *aead = crypto_aead_reqtfm(areq);
-	int ivsize = crypto_aead_ivsize(aead);
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	int ivsize = crypto_aead_ivsize(aead);
 
 	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
 #endif
-	edesc = (struct ipsec_esp_edesc *)((char *)desc -
-		 offsetof(struct ipsec_esp_edesc, hw_desc));
+
+	edesc = (struct aead_edesc *)((char *)desc -
+		 offsetof(struct aead_edesc, hw_desc));
 
 	if (err) {
 		char tmp[CAAM_ERROR_STR_MAX];
@@ -444,39 +916,50 @@ static void ipsec_esp_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
 		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
 	}
 
-	ipsec_esp_unmap(jrdev, edesc, areq);
+	aead_unmap(jrdev, edesc, req);
 
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "assoc  @"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(areq->assoc),
-		       areq->assoclen , 1);
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->assoc),
+		       req->assoclen , 1);
 	print_hex_dump(KERN_ERR, "dstiv  @"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(areq->src) - ivsize,
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src) - ivsize,
 		       edesc->src_nents ? 100 : ivsize, 1);
 	print_hex_dump(KERN_ERR, "dst    @"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(areq->src),
-		       edesc->src_nents ? 100 : areq->cryptlen +
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
+		       edesc->src_nents ? 100 : req->cryptlen +
 		       ctx->authsize + 4, 1);
 #endif
 
 	kfree(edesc);
 
-	aead_request_complete(areq, err);
+	aead_request_complete(req, err);
 }
 
-static void ipsec_esp_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
+static void aead_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 				   void *context)
 {
-	struct aead_request *areq = context;
-	struct ipsec_esp_edesc *edesc;
+	struct aead_request *req = context;
+	struct aead_edesc *edesc;
 #ifdef DEBUG
-	struct crypto_aead *aead = crypto_aead_reqtfm(areq);
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	int ivsize = crypto_aead_ivsize(aead);
 
 	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
 #endif
-	edesc = (struct ipsec_esp_edesc *)((char *)desc -
-		 offsetof(struct ipsec_esp_edesc, hw_desc));
+
+	edesc = (struct aead_edesc *)((char *)desc -
+		 offsetof(struct aead_edesc, hw_desc));
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "dstiv  @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, req->iv,
+		       ivsize, 1);
+	print_hex_dump(KERN_ERR, "dst    @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->dst),
+		       req->cryptlen, 1);
+#endif
 
 	if (err) {
 		char tmp[CAAM_ERROR_STR_MAX];
@@ -484,7 +967,7 @@ static void ipsec_esp_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
 	}
 
-	ipsec_esp_unmap(jrdev, edesc, areq);
+	aead_unmap(jrdev, edesc, req);
 
 	/*
 	 * verify hw auth check passed else return -EBADMSG
@@ -495,255 +978,423 @@ static void ipsec_esp_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "iphdrout@"xstr(__LINE__)": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4,
-		       ((char *)sg_virt(areq->assoc) - sizeof(struct iphdr)),
-		       sizeof(struct iphdr) + areq->assoclen +
-		       ((areq->cryptlen > 1500) ? 1500 : areq->cryptlen) +
+		       ((char *)sg_virt(req->assoc) - sizeof(struct iphdr)),
+		       sizeof(struct iphdr) + req->assoclen +
+		       ((req->cryptlen > 1500) ? 1500 : req->cryptlen) +
 		       ctx->authsize + 36, 1);
 	if (!err && edesc->link_tbl_bytes) {
-		struct scatterlist *sg = sg_last(areq->src, edesc->src_nents);
+		struct scatterlist *sg = sg_last(req->src, edesc->src_nents);
 		print_hex_dump(KERN_ERR, "sglastout@"xstr(__LINE__)": ",
 			       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(sg),
 			sg->length + ctx->authsize + 16, 1);
 	}
 #endif
+
 	kfree(edesc);
 
-	aead_request_complete(areq, err);
+	aead_request_complete(req, err);
+}
+
+static void ablkcipher_encrypt_done(struct device *jrdev, u32 *desc, u32 err,
+				   void *context)
+{
+	struct ablkcipher_request *req = context;
+	struct ablkcipher_edesc *edesc;
+#ifdef DEBUG
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	int ivsize = crypto_ablkcipher_ivsize(ablkcipher);
+
+	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
+#endif
+
+	edesc = (struct ablkcipher_edesc *)((char *)desc -
+		 offsetof(struct ablkcipher_edesc, hw_desc));
+
+	if (err) {
+		char tmp[CAAM_ERROR_STR_MAX];
+
+		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
+	}
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "dstiv  @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, req->info,
+		       edesc->src_nents > 1 ? 100 : ivsize, 1);
+	print_hex_dump(KERN_ERR, "dst    @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
+		       edesc->dst_nents > 1 ? 100 : req->nbytes, 1);
+#endif
+
+	ablkcipher_unmap(jrdev, edesc, req);
+	kfree(edesc);
+
+	ablkcipher_request_complete(req, err);
+}
+
+static void ablkcipher_decrypt_done(struct device *jrdev, u32 *desc, u32 err,
+				    void *context)
+{
+	struct ablkcipher_request *req = context;
+	struct ablkcipher_edesc *edesc;
+#ifdef DEBUG
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	int ivsize = crypto_ablkcipher_ivsize(ablkcipher);
+
+	dev_err(jrdev, "%s %d: err 0x%x\n", __func__, __LINE__, err);
+#endif
+
+	edesc = (struct ablkcipher_edesc *)((char *)desc -
+		 offsetof(struct ablkcipher_edesc, hw_desc));
+	if (err) {
+		char tmp[CAAM_ERROR_STR_MAX];
+
+		dev_err(jrdev, "%08x: %s\n", err, caam_jr_strstatus(tmp, err));
+	}
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "dstiv  @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, req->info,
+		       ivsize, 1);
+	print_hex_dump(KERN_ERR, "dst    @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
+		       edesc->dst_nents > 1 ? 100 : req->nbytes, 1);
+#endif
+
+	ablkcipher_unmap(jrdev, edesc, req);
+	kfree(edesc);
+
+	ablkcipher_request_complete(req, err);
+}
+
+static void sg_to_link_tbl_one(struct link_tbl_entry *link_tbl_ptr,
+			       dma_addr_t dma, u32 len, u32 offset)
+{
+#ifndef CONFIG_64BIT
+	link_tbl_ptr->reserved = 0;	/* ensure high half is zero */
+#endif
+	link_tbl_ptr->ptr = dma;
+	link_tbl_ptr->len = (len & LINKTBL_LEN_MASK);
+	/* Does not add in buffer pool ID's at this time */
+	link_tbl_ptr->bpid_offset = (offset & LINKTBL_OFFS_MASK);
+
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "link_tbl_ptr@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, link_tbl_ptr,
+		       sizeof(struct link_tbl_entry), 1);
+#endif
 }
 
 /*
  * convert scatterlist to h/w link table format
- * scatterlist must have been previously dma mapped
+ * but does not have final bit; instead, returns last entry
  */
-static void sg_to_link_tbl(struct scatterlist *sg, int sg_count,
-			   struct link_tbl_entry *link_tbl_ptr, u32 offset)
+static struct link_tbl_entry *sg_to_link_tbl(struct scatterlist *sg,
+					     int sg_count, struct link_tbl_entry
+					     *link_tbl_ptr, u32 offset)
 {
 	while (sg_count) {
-		link_tbl_ptr->ptr = sg_dma_address(sg);
-		link_tbl_ptr->len = sg_dma_len(sg);
-		link_tbl_ptr->reserved = 0;
-		link_tbl_ptr->buf_pool_id = 0;
-		link_tbl_ptr->offset = offset;
+		sg_to_link_tbl_one(link_tbl_ptr, sg_dma_address(sg),
+				   sg_dma_len(sg), offset);
 		link_tbl_ptr++;
 		sg = sg_next(sg);
 		sg_count--;
 	}
+	return link_tbl_ptr - 1;
+}
 
-	/* set Final bit (marks end of link table) */
-	link_tbl_ptr--;
-	link_tbl_ptr->len |= 0x40000000;
+/*
+ * convert scatterlist to h/w link table format
+ * scatterlist must have been previously dma mapped
+ */
+static void sg_to_link_tbl_last(struct scatterlist *sg, int sg_count,
+				struct link_tbl_entry *link_tbl_ptr, u32 offset)
+{
+	link_tbl_ptr = sg_to_link_tbl(sg, sg_count, link_tbl_ptr, offset);
+	link_tbl_ptr->len |= LINKTBL_FINAL;
 }
 
 /*
- * fill in and submit ipsec_esp job descriptor
+ * Fill in aead job descriptor
  */
-static int ipsec_esp(struct ipsec_esp_edesc *edesc, struct aead_request *areq,
-		     u32 encrypt,
-		     void (*callback) (struct device *dev, u32 *desc,
-				       u32 err, void *context))
+static void init_aead_job(u32 *sh_desc, dma_addr_t ptr,
+			  struct aead_edesc *edesc,
+			  struct aead_request *req,
+			  bool all_contig, bool encrypt)
 {
-	struct crypto_aead *aead = crypto_aead_reqtfm(areq);
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
-	struct device *jrdev = ctx->jrdev;
-	u32 *desc = edesc->hw_desc, options;
-	int ret, sg_count, assoc_sg_count;
 	int ivsize = crypto_aead_ivsize(aead);
 	int authsize = ctx->authsize;
-	dma_addr_t ptr, dst_dma, src_dma;
-#ifdef DEBUG
-	u32 *sh_desc = ctx->sh_desc;
+	u32 *desc = edesc->hw_desc;
+	u32 out_options = 0, in_options;
+	dma_addr_t dst_dma, src_dma;
+	int len, link_tbl_index = 0;
 
+#ifdef DEBUG
 	debug("assoclen %d cryptlen %d authsize %d\n",
-	      areq->assoclen, areq->cryptlen, authsize);
+	      req->assoclen, req->cryptlen, authsize);
 	print_hex_dump(KERN_ERR, "assoc  @"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(areq->assoc),
-		       areq->assoclen , 1);
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->assoc),
+		       req->assoclen , 1);
 	print_hex_dump(KERN_ERR, "presciv@"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(areq->src) - ivsize,
+		       DUMP_PREFIX_ADDRESS, 16, 4, req->iv,
 		       edesc->src_nents ? 100 : ivsize, 1);
 	print_hex_dump(KERN_ERR, "src    @"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(areq->src),
-			edesc->src_nents ? 100 : areq->cryptlen + authsize, 1);
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
+			edesc->src_nents ? 100 : req->cryptlen, 1);
 	print_hex_dump(KERN_ERR, "shrdesc@"xstr(__LINE__)": ",
 		       DUMP_PREFIX_ADDRESS, 16, 4, sh_desc,
 		       desc_bytes(sh_desc), 1);
 #endif
-	assoc_sg_count = dma_map_sg(jrdev, areq->assoc, edesc->assoc_nents ?: 1,
-				    DMA_TO_DEVICE);
-	if (areq->src == areq->dst)
-		sg_count = dma_map_sg(jrdev, areq->src, edesc->src_nents ? : 1,
-				      DMA_BIDIRECTIONAL);
-	else
-		sg_count = dma_map_sg(jrdev, areq->src, edesc->src_nents ? : 1,
-				      DMA_TO_DEVICE);
 
-	/* start auth operation */
-	append_operation(desc, ctx->class2_alg_type | OP_ALG_AS_INITFINAL |
-			 (encrypt ? : OP_ALG_ICV_ON));
+	len = desc_len(sh_desc);
+	init_job_desc_shared(desc, ptr, len, HDR_SHARE_DEFER | HDR_REVERSE);
 
-	/* Load FIFO with data for Class 2 CHA */
-	options = FIFOLD_CLASS_CLASS2 | FIFOLD_TYPE_MSG;
-	if (!edesc->assoc_nents) {
-		ptr = sg_dma_address(areq->assoc);
+	if (all_contig) {
+		src_dma = sg_dma_address(req->assoc);
+		in_options = 0;
 	} else {
-		sg_to_link_tbl(areq->assoc, edesc->assoc_nents,
-			       edesc->link_tbl, 0);
-		ptr = edesc->link_tbl_dma;
-		options |= LDST_SGF;
+		src_dma = edesc->link_tbl_dma;
+		link_tbl_index += (edesc->assoc_nents ? : 1) + 1 +
+				  (edesc->src_nents ? : 1);
+		in_options = LDST_SGF;
 	}
-	append_fifo_load(desc, ptr, areq->assoclen, options);
-
-	/* copy iv from cipher/class1 input context to class2 infifo */
-	append_move(desc, MOVE_SRC_CLASS1CTX | MOVE_DEST_CLASS2INFIFO | ivsize);
-
-	if (!encrypt) {
-		u32 *jump_cmd, *uncond_jump_cmd;
-
-		/* JUMP if shared */
-		jump_cmd = append_jump(desc, JUMP_TEST_ALL | JUMP_COND_SHRD);
+	if (encrypt)
+		append_seq_in_ptr(desc, src_dma, req->assoclen + ivsize +
+				  req->cryptlen - authsize, in_options);
+	else
+		append_seq_in_ptr(desc, src_dma, req->assoclen + ivsize +
+				  req->cryptlen, in_options);
 
-		/* start class 1 (cipher) operation, non-shared version */
-		append_operation(desc, ctx->class1_alg_type |
-				 OP_ALG_AS_INITFINAL);
+	if (likely(req->src == req->dst)) {
+		if (all_contig) {
+			dst_dma = sg_dma_address(req->src);
+		} else {
+			dst_dma = src_dma + sizeof(struct link_tbl_entry) *
+				  ((edesc->assoc_nents ? : 1) + 1);
+			out_options = LDST_SGF;
+		}
+	} else {
+		if (!edesc->dst_nents) {
+			dst_dma = sg_dma_address(req->dst);
+		} else {
+			dst_dma = edesc->link_tbl_dma +
+				  link_tbl_index *
+				  sizeof(struct link_tbl_entry);
+			out_options = LDST_SGF;
+		}
+	}
+	if (encrypt)
+		append_seq_out_ptr(desc, dst_dma, req->cryptlen, out_options);
+	else
+		append_seq_out_ptr(desc, dst_dma, req->cryptlen - authsize,
+				   out_options);
+}
 
-		uncond_jump_cmd = append_jump(desc, 0);
+/*
+ * Fill in aead givencrypt job descriptor
+ */
+static void init_aead_giv_job(u32 *sh_desc, dma_addr_t ptr,
+			      struct aead_edesc *edesc,
+			      struct aead_request *req,
+			      int contig)
+{
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	int ivsize = crypto_aead_ivsize(aead);
+	int authsize = ctx->authsize;
+	u32 *desc = edesc->hw_desc;
+	u32 out_options = 0, in_options;
+	dma_addr_t dst_dma, src_dma;
+	int len, link_tbl_index = 0;
 
-		set_jump_tgt_here(desc, jump_cmd);
+#ifdef DEBUG
+	debug("assoclen %d cryptlen %d authsize %d\n",
+	      req->assoclen, req->cryptlen, authsize);
+	print_hex_dump(KERN_ERR, "assoc  @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->assoc),
+		       req->assoclen , 1);
+	print_hex_dump(KERN_ERR, "presciv@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, req->iv, ivsize, 1);
+	print_hex_dump(KERN_ERR, "src    @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
+			edesc->src_nents > 1 ? 100 : req->cryptlen, 1);
+	print_hex_dump(KERN_ERR, "shrdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sh_desc,
+		       desc_bytes(sh_desc), 1);
+#endif
 
-		/* start class 1 (cipher) operation, shared version */
-		append_operation(desc, ctx->class1_alg_type |
-				 OP_ALG_AS_INITFINAL | OP_ALG_AAI_DK);
-		set_jump_tgt_here(desc, uncond_jump_cmd);
-	} else
-		append_operation(desc, ctx->class1_alg_type |
-				 OP_ALG_AS_INITFINAL | encrypt);
+	len = desc_len(sh_desc);
+	init_job_desc_shared(desc, ptr, len, HDR_SHARE_DEFER | HDR_REVERSE);
 
-	/* load payload & instruct to class2 to snoop class 1 if encrypting */
-	options = 0;
-	if (!edesc->src_nents) {
-		src_dma = sg_dma_address(areq->src);
+	if (contig & GIV_SRC_CONTIG) {
+		src_dma = sg_dma_address(req->assoc);
+		in_options = 0;
 	} else {
-		sg_to_link_tbl(areq->src, edesc->src_nents, edesc->link_tbl +
-			       edesc->assoc_nents, 0);
-		src_dma = edesc->link_tbl_dma + edesc->assoc_nents *
-			  sizeof(struct link_tbl_entry);
-		options |= LDST_SGF;
+		src_dma = edesc->link_tbl_dma;
+		link_tbl_index += edesc->assoc_nents + 1 + edesc->src_nents;
+		in_options = LDST_SGF;
 	}
-	append_seq_in_ptr(desc, src_dma, areq->cryptlen + authsize, options);
-	append_seq_fifo_load(desc, areq->cryptlen, FIFOLD_CLASS_BOTH |
-			     FIFOLD_TYPE_LASTBOTH |
-			     (encrypt ? FIFOLD_TYPE_MSG1OUT2
-				      : FIFOLD_TYPE_MSG));
-
-	/* specify destination */
-	if (areq->src == areq->dst) {
-		dst_dma = src_dma;
+	append_seq_in_ptr(desc, src_dma, req->assoclen + ivsize +
+			  req->cryptlen - authsize, in_options);
+
+	if (contig & GIV_DST_CONTIG) {
+		dst_dma = edesc->iv_dma;
 	} else {
-		sg_count = dma_map_sg(jrdev, areq->dst, edesc->dst_nents ? : 1,
-				      DMA_FROM_DEVICE);
-		if (!edesc->dst_nents) {
-			dst_dma = sg_dma_address(areq->dst);
-			options = 0;
+		if (likely(req->src == req->dst)) {
+			dst_dma = src_dma + sizeof(struct link_tbl_entry) *
+				  edesc->assoc_nents;
+			out_options = LDST_SGF;
 		} else {
-			sg_to_link_tbl(areq->dst, edesc->dst_nents,
-				       edesc->link_tbl + edesc->assoc_nents +
-				       edesc->src_nents, 0);
-			dst_dma = edesc->link_tbl_dma + (edesc->assoc_nents +
-				  edesc->src_nents) *
+			dst_dma = edesc->link_tbl_dma +
+				  link_tbl_index *
 				  sizeof(struct link_tbl_entry);
-			options = LDST_SGF;
+			out_options = LDST_SGF;
 		}
 	}
-	append_seq_out_ptr(desc, dst_dma, areq->cryptlen + authsize, options);
-	append_seq_fifo_store(desc, areq->cryptlen, FIFOST_TYPE_MESSAGE_DATA);
 
-	/* ICV */
-	if (encrypt)
-		append_seq_store(desc, authsize, LDST_CLASS_2_CCB |
-				 LDST_SRCDST_BYTE_CONTEXT);
-	else
-		append_seq_fifo_load(desc, authsize, FIFOLD_CLASS_CLASS2 |
-				     FIFOLD_TYPE_LAST2 | FIFOLD_TYPE_ICV);
+	append_seq_out_ptr(desc, dst_dma, ivsize + req->cryptlen, out_options);
+}
+
+/*
+ * Fill in ablkcipher job descriptor
+ */
+static void init_ablkcipher_job(u32 *sh_desc, dma_addr_t ptr,
+				struct ablkcipher_edesc *edesc,
+				struct ablkcipher_request *req,
+				bool iv_contig)
+{
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	int ivsize = crypto_ablkcipher_ivsize(ablkcipher);
+	u32 *desc = edesc->hw_desc;
+	u32 out_options = 0, in_options;
+	dma_addr_t dst_dma, src_dma;
+	int len, link_tbl_index = 0;
 
 #ifdef DEBUG
-	debug("job_desc_len %d\n", desc_len(desc));
-	print_hex_dump(KERN_ERR, "jobdesc@"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, desc, desc_bytes(desc) , 1);
-	print_hex_dump(KERN_ERR, "jdlinkt@"xstr(__LINE__)": ",
-		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->link_tbl,
-			edesc->link_tbl_bytes, 1);
+	print_hex_dump(KERN_ERR, "presciv@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, req->info,
+		       ivsize, 1);
+	print_hex_dump(KERN_ERR, "src    @"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
+		       edesc->src_nents ? 100 : req->nbytes, 1);
 #endif
 
-	ret = caam_jr_enqueue(jrdev, desc, callback, areq);
-	if (!ret)
-		ret = -EINPROGRESS;
-	else {
-		ipsec_esp_unmap(jrdev, edesc, areq);
-		kfree(edesc);
+	len = desc_len(sh_desc);
+	init_job_desc_shared(desc, ptr, len, HDR_SHARE_DEFER | HDR_REVERSE);
+
+	if (iv_contig) {
+		src_dma = edesc->iv_dma;
+		in_options = 0;
+	} else {
+		src_dma = edesc->link_tbl_dma;
+		link_tbl_index += (iv_contig ? 0 : 1) + edesc->src_nents;
+		in_options = LDST_SGF;
 	}
+	append_seq_in_ptr(desc, src_dma, req->nbytes + ivsize, in_options);
 
-	return ret;
+	if (likely(req->src == req->dst)) {
+		if (!edesc->src_nents && iv_contig) {
+			dst_dma = sg_dma_address(req->src);
+		} else {
+			dst_dma = edesc->link_tbl_dma +
+				sizeof(struct link_tbl_entry);
+			out_options = LDST_SGF;
+		}
+	} else {
+		if (!edesc->dst_nents) {
+			dst_dma = sg_dma_address(req->dst);
+		} else {
+			dst_dma = edesc->link_tbl_dma +
+				link_tbl_index * sizeof(struct link_tbl_entry);
+			out_options = LDST_SGF;
+		}
+	}
+	append_seq_out_ptr(desc, dst_dma, req->nbytes, out_options);
 }
 
 /*
  * derive number of elements in scatterlist
  */
-static int sg_count(struct scatterlist *sg_list, int nbytes, int *chained)
+static int sg_count(struct scatterlist *sg_list, int nbytes)
 {
 	struct scatterlist *sg = sg_list;
 	int sg_nents = 0;
 
-	*chained = 0;
 	while (nbytes > 0) {
 		sg_nents++;
 		nbytes -= sg->length;
 		if (!sg_is_last(sg) && (sg + 1)->length == 0)
-			*chained = 1;
+			BUG(); /* Not support chaining */
 		sg = scatterwalk_sg_next(sg);
 	}
 
+	if (likely(sg_nents == 1))
+		return 0;
+
 	return sg_nents;
 }
 
 /*
- * allocate and map the ipsec_esp extended descriptor
+ * allocate and map the aead extended descriptor
  */
-static struct ipsec_esp_edesc *ipsec_esp_edesc_alloc(struct aead_request *areq,
-						     int desc_bytes)
+static struct aead_edesc *aead_edesc_alloc(struct aead_request *req,
+					   int desc_bytes, bool *all_contig_ptr)
 {
-	struct crypto_aead *aead = crypto_aead_reqtfm(areq);
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
 	struct device *jrdev = ctx->jrdev;
-	gfp_t flags = areq->base.flags & CRYPTO_TFM_REQ_MAY_SLEEP ? GFP_KERNEL :
-		      GFP_ATOMIC;
-	int assoc_nents, src_nents, dst_nents = 0, chained, link_tbl_bytes;
-	struct ipsec_esp_edesc *edesc;
-
-	assoc_nents = sg_count(areq->assoc, areq->assoclen, &chained);
-	BUG_ON(chained);
-	if (likely(assoc_nents == 1))
-		assoc_nents = 0;
-
-	src_nents = sg_count(areq->src, areq->cryptlen + ctx->authsize,
-			     &chained);
-	BUG_ON(chained);
-	if (src_nents == 1)
-		src_nents = 0;
-
-	if (unlikely(areq->dst != areq->src)) {
-		dst_nents = sg_count(areq->dst, areq->cryptlen + ctx->authsize,
-				     &chained);
-		BUG_ON(chained);
-		if (dst_nents == 1)
-			dst_nents = 0;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	int assoc_nents, src_nents, dst_nents = 0;
+	struct aead_edesc *edesc;
+	dma_addr_t iv_dma = 0;
+	int sgc;
+	bool all_contig = true;
+	int ivsize = crypto_aead_ivsize(aead);
+	int link_tbl_index, link_tbl_len = 0, link_tbl_bytes;
+
+	assoc_nents = sg_count(req->assoc, req->assoclen);
+	src_nents = sg_count(req->src, req->cryptlen);
+
+	if (unlikely(req->dst != req->src))
+		dst_nents = sg_count(req->dst, req->cryptlen);
+
+	sgc = dma_map_sg(jrdev, req->assoc, assoc_nents ? : 1,
+			 DMA_BIDIRECTIONAL);
+	dma_sync_sg_for_device(jrdev, req->assoc, sgc,
+			       DMA_BIDIRECTIONAL);
+	if (likely(req->src == req->dst)) {
+		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
+				 DMA_BIDIRECTIONAL);
+		dma_sync_sg_for_device(jrdev, req->src, sgc,
+				       DMA_BIDIRECTIONAL);
+	} else {
+		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
+				 DMA_TO_DEVICE);
+		dma_sync_sg_for_device(jrdev, req->src, sgc,
+				       DMA_TO_DEVICE);
+		sgc = dma_map_sg(jrdev, req->dst, dst_nents ? : 1,
+				 DMA_FROM_DEVICE);
 	}
 
-	link_tbl_bytes = (assoc_nents + src_nents + dst_nents) *
-			 sizeof(struct link_tbl_entry);
-	debug("link_tbl_bytes %d\n", link_tbl_bytes);
+	/* Check if data are contiguous */
+	iv_dma = dma_map_single(jrdev, req->iv, ivsize, DMA_TO_DEVICE);
+	if (assoc_nents || sg_dma_address(req->assoc) + req->assoclen !=
+	    iv_dma || src_nents || iv_dma + ivsize !=
+	    sg_dma_address(req->src)) {
+		all_contig = false;
+		assoc_nents = assoc_nents ? : 1;
+		src_nents = src_nents ? : 1;
+		link_tbl_len = assoc_nents + 1 + src_nents;
+	}
+	link_tbl_len += dst_nents;
+
+	link_tbl_bytes = link_tbl_len * sizeof(struct link_tbl_entry);
+	dma_sync_single_for_device(jrdev, iv_dma, ivsize, DMA_TO_DEVICE);
 
 	/* allocate space for base edesc and hw desc commands, link tables */
-	edesc = kmalloc(sizeof(struct ipsec_esp_edesc) + desc_bytes +
+	edesc = kmalloc(sizeof(struct aead_edesc) + desc_bytes +
 			link_tbl_bytes, GFP_DMA | flags);
 	if (!edesc) {
 		dev_err(jrdev, "could not allocate extended descriptor\n");
@@ -753,142 +1404,470 @@ static struct ipsec_esp_edesc *ipsec_esp_edesc_alloc(struct aead_request *areq,
 	edesc->assoc_nents = assoc_nents;
 	edesc->src_nents = src_nents;
 	edesc->dst_nents = dst_nents;
-	edesc->link_tbl = (void *)edesc + sizeof(struct ipsec_esp_edesc) +
+	edesc->iv_dma = iv_dma;
+	edesc->link_tbl_bytes = link_tbl_bytes;
+	edesc->link_tbl = (void *)edesc + sizeof(struct aead_edesc) +
 			  desc_bytes;
 	edesc->link_tbl_dma = dma_map_single(jrdev, edesc->link_tbl,
 					     link_tbl_bytes, DMA_TO_DEVICE);
-	edesc->link_tbl_bytes = link_tbl_bytes;
+	*all_contig_ptr = all_contig;
+
+	link_tbl_index = 0;
+	if (!all_contig) {
+		sg_to_link_tbl(req->assoc,
+			       (assoc_nents ? : 1),
+			       edesc->link_tbl +
+			       link_tbl_index, 0);
+		link_tbl_index += assoc_nents ? : 1;
+		sg_to_link_tbl_one(edesc->link_tbl + link_tbl_index,
+				   iv_dma, ivsize, 0);
+		link_tbl_index += 1;
+		sg_to_link_tbl_last(req->src,
+				    (src_nents ? : 1),
+				    edesc->link_tbl +
+				    link_tbl_index, 0);
+		link_tbl_index += src_nents ? : 1;
+	}
+	if (dst_nents) {
+		sg_to_link_tbl_last(req->dst, dst_nents,
+				    edesc->link_tbl + link_tbl_index, 0);
+	}
+	dma_sync_single_for_device(jrdev, edesc->link_tbl_dma, link_tbl_bytes,
+				   DMA_TO_DEVICE);
 
 	return edesc;
 }
 
-static int aead_authenc_encrypt(struct aead_request *areq)
+static int aead_encrypt(struct aead_request *req)
 {
-	struct ipsec_esp_edesc *edesc;
-	struct crypto_aead *aead = crypto_aead_reqtfm(areq);
+	struct aead_edesc *edesc;
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
 	struct device *jrdev = ctx->jrdev;
-	int ivsize = crypto_aead_ivsize(aead);
+	bool all_contig;
 	u32 *desc;
-	dma_addr_t iv_dma;
+	int ret = 0;
+
+	req->cryptlen += ctx->authsize;
 
 	/* allocate extended descriptor */
-	edesc = ipsec_esp_edesc_alloc(areq, DESC_AEAD_ENCRYPT_TEXT_LEN *
-				      CAAM_CMD_SZ);
+	edesc = aead_edesc_alloc(req, DESC_JOB_IO_LEN *
+				 CAAM_CMD_SZ, &all_contig);
 	if (IS_ERR(edesc))
 		return PTR_ERR(edesc);
 
-	desc = edesc->hw_desc;
-
-	/* insert shared descriptor pointer */
-	init_job_desc_shared(desc, ctx->shared_desc_phys,
-			     desc_len(ctx->sh_desc), HDR_SHARE_DEFER);
-
-	iv_dma = dma_map_single(jrdev, areq->iv, ivsize, DMA_TO_DEVICE);
-	/* check dma error */
+	/* Create and submit job descriptor */
+	init_aead_job(ctx->sh_desc_enc, ctx->sh_desc_enc_dma, edesc, req,
+		      all_contig, true);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "aead jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->hw_desc,
+		       desc_bytes(edesc->hw_desc), 1);
+#endif
 
-	append_load(desc, iv_dma, ivsize,
-		    LDST_CLASS_1_CCB | LDST_SRCDST_BYTE_CONTEXT);
+	desc = edesc->hw_desc;
+	ret = caam_jr_enqueue(jrdev, desc, aead_encrypt_done, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		aead_unmap(jrdev, edesc, req);
+		kfree(edesc);
+	}
 
-	return ipsec_esp(edesc, areq, OP_ALG_ENCRYPT, ipsec_esp_encrypt_done);
+	return ret;
 }
 
-static int aead_authenc_decrypt(struct aead_request *req)
+static int aead_decrypt(struct aead_request *req)
 {
+	struct aead_edesc *edesc;
 	struct crypto_aead *aead = crypto_aead_reqtfm(req);
-	int ivsize = crypto_aead_ivsize(aead);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
 	struct device *jrdev = ctx->jrdev;
-	struct ipsec_esp_edesc *edesc;
+	bool all_contig;
 	u32 *desc;
-	dma_addr_t iv_dma;
-
-	req->cryptlen -= ctx->authsize;
+	int ret = 0;
 
 	/* allocate extended descriptor */
-	edesc = ipsec_esp_edesc_alloc(req, DESC_AEAD_DECRYPT_TEXT_LEN *
-				      CAAM_CMD_SZ);
+	edesc = aead_edesc_alloc(req, DESC_JOB_IO_LEN *
+				 CAAM_CMD_SZ, &all_contig);
 	if (IS_ERR(edesc))
 		return PTR_ERR(edesc);
 
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "dec src@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
+		       req->cryptlen, 1);
+#endif
+
+	/* Create and submit job descriptor*/
+	init_aead_job(ctx->sh_desc_dec,
+		      ctx->sh_desc_dec_dma, edesc, req, all_contig, false);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "aead jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->hw_desc,
+		       desc_bytes(edesc->hw_desc), 1);
+#endif
+
 	desc = edesc->hw_desc;
+	ret = caam_jr_enqueue(jrdev, desc, aead_decrypt_done, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		aead_unmap(jrdev, edesc, req);
+		kfree(edesc);
+	}
 
-	/* insert shared descriptor pointer */
-	init_job_desc_shared(desc, ctx->shared_desc_phys,
-			     desc_len(ctx->sh_desc), HDR_SHARE_DEFER);
+	return ret;
+}
 
-	iv_dma = dma_map_single(jrdev, req->iv, ivsize, DMA_TO_DEVICE);
-	/* check dma error */
+/*
+ * allocate and map the aead extended descriptor for aead givencrypt
+ */
+static struct aead_edesc *aead_giv_edesc_alloc(struct aead_givcrypt_request
+					       *greq, int desc_bytes,
+					       u32 *contig_ptr)
+{
+	struct aead_request *req = &greq->areq;
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
+	struct caam_ctx *ctx = crypto_aead_ctx(aead);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
+	int assoc_nents, src_nents, dst_nents = 0;
+	struct aead_edesc *edesc;
+	dma_addr_t iv_dma = 0;
+	int sgc;
+	u32 contig = GIV_SRC_CONTIG | GIV_DST_CONTIG;
+	int ivsize = crypto_aead_ivsize(aead);
+	int link_tbl_index, link_tbl_len = 0, link_tbl_bytes;
+
+	assoc_nents = sg_count(req->assoc, req->assoclen);
+	src_nents = sg_count(req->src, req->cryptlen);
+
+	if (unlikely(req->dst != req->src))
+		dst_nents = sg_count(req->dst, req->cryptlen);
+
+	sgc = dma_map_sg(jrdev, req->assoc, assoc_nents ? : 1,
+			 DMA_BIDIRECTIONAL);
+	dma_sync_sg_for_device(jrdev, req->assoc, assoc_nents ? : 1,
+			       DMA_BIDIRECTIONAL);
+	if (likely(req->src == req->dst)) {
+		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
+				 DMA_BIDIRECTIONAL);
+		dma_sync_sg_for_device(jrdev, req->src, src_nents ? : 1,
+				       DMA_BIDIRECTIONAL);
+	} else {
+		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
+				 DMA_TO_DEVICE);
+		dma_sync_sg_for_device(jrdev, req->src, src_nents ? : 1,
+				    DMA_TO_DEVICE);
+		sgc = dma_map_sg(jrdev, req->dst, dst_nents ? : 1,
+				 DMA_FROM_DEVICE);
+	}
 
-	append_load(desc, iv_dma, ivsize,
-		    LDST_CLASS_1_CCB | LDST_SRCDST_BYTE_CONTEXT);
+	/* Check if data are contiguous */
+	iv_dma = dma_map_single(jrdev, greq->giv, ivsize, DMA_TO_DEVICE);
+	if (assoc_nents || sg_dma_address(req->assoc) + req->assoclen !=
+	    iv_dma || src_nents || iv_dma + ivsize != sg_dma_address(req->src))
+		contig &= ~GIV_SRC_CONTIG;
+	if (dst_nents || iv_dma + ivsize != sg_dma_address(req->dst))
+		contig &= ~GIV_DST_CONTIG;
+		if (unlikely(req->src != req->dst)) {
+			dst_nents = dst_nents ? : 1;
+			link_tbl_len += 1;
+		}
+	if (!(contig & GIV_SRC_CONTIG)) {
+		assoc_nents = assoc_nents ? : 1;
+		src_nents = src_nents ? : 1;
+		link_tbl_len += assoc_nents + 1 + src_nents;
+		if (likely(req->src == req->dst))
+			contig &= ~GIV_DST_CONTIG;
+	}
+	link_tbl_len += dst_nents;
+
+	link_tbl_bytes = link_tbl_len * sizeof(struct link_tbl_entry);
+
+	dma_sync_single_for_device(jrdev, iv_dma, ivsize, DMA_TO_DEVICE);
+
+	/* allocate space for base edesc and hw desc commands, link tables */
+	edesc = kmalloc(sizeof(struct aead_edesc) + desc_bytes +
+			link_tbl_bytes, GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(jrdev, "could not allocate extended descriptor\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	edesc->assoc_nents = assoc_nents;
+	edesc->src_nents = src_nents;
+	edesc->dst_nents = dst_nents;
+	edesc->iv_dma = iv_dma;
+	edesc->link_tbl_bytes = link_tbl_bytes;
+	edesc->link_tbl = (void *)edesc + sizeof(struct aead_edesc) +
+			  desc_bytes;
+	edesc->link_tbl_dma = dma_map_single(jrdev, edesc->link_tbl,
+					     link_tbl_bytes, DMA_TO_DEVICE);
+	*contig_ptr = contig;
+
+	link_tbl_index = 0;
+	if (!(contig & GIV_SRC_CONTIG)) {
+		sg_to_link_tbl(req->assoc, assoc_nents,
+			       edesc->link_tbl +
+			       link_tbl_index, 0);
+		link_tbl_index += assoc_nents;
+		sg_to_link_tbl_one(edesc->link_tbl + link_tbl_index,
+				   iv_dma, ivsize, 0);
+		link_tbl_index += 1;
+		sg_to_link_tbl_last(req->src, src_nents,
+				    edesc->link_tbl +
+				    link_tbl_index, 0);
+		link_tbl_index += src_nents;
+	}
+	if (unlikely(req->src != req->dst && !(contig & GIV_DST_CONTIG))) {
+		sg_to_link_tbl_one(edesc->link_tbl + link_tbl_index,
+				   iv_dma, ivsize, 0);
+		link_tbl_index += 1;
+		sg_to_link_tbl_last(req->dst, dst_nents,
+				    edesc->link_tbl + link_tbl_index, 0);
+	}
+	dma_sync_single_for_device(jrdev, edesc->link_tbl_dma, link_tbl_bytes,
+				   DMA_TO_DEVICE);
 
-	return ipsec_esp(edesc, req, !OP_ALG_ENCRYPT, ipsec_esp_decrypt_done);
+	return edesc;
 }
 
-static int aead_authenc_givencrypt(struct aead_givcrypt_request *req)
+static int aead_givencrypt(struct aead_givcrypt_request *areq)
 {
-	struct aead_request *areq = &req->areq;
-	struct ipsec_esp_edesc *edesc;
-	struct crypto_aead *aead = crypto_aead_reqtfm(areq);
+	struct aead_request *req = &areq->areq;
+	struct aead_edesc *edesc;
+	struct crypto_aead *aead = crypto_aead_reqtfm(req);
 	struct caam_ctx *ctx = crypto_aead_ctx(aead);
 	struct device *jrdev = ctx->jrdev;
-	int ivsize = crypto_aead_ivsize(aead);
-	dma_addr_t iv_dma;
+	u32 contig;
 	u32 *desc;
+	int ret = 0;
 
-	iv_dma = dma_map_single(jrdev, req->giv, ivsize, DMA_FROM_DEVICE);
-
-	debug("%s: giv %p\n", __func__, req->giv);
+	req->cryptlen += ctx->authsize;
 
 	/* allocate extended descriptor */
-	edesc = ipsec_esp_edesc_alloc(areq, DESC_AEAD_GIVENCRYPT_TEXT_LEN *
-				      CAAM_CMD_SZ);
+	edesc = aead_giv_edesc_alloc(areq, DESC_JOB_IO_LEN *
+				     CAAM_CMD_SZ, &contig);
+
 	if (IS_ERR(edesc))
 		return PTR_ERR(edesc);
 
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "giv src@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, sg_virt(req->src),
+		       req->cryptlen, 1);
+#endif
+
+	/* Create and submit job descriptor*/
+	init_aead_giv_job(ctx->sh_desc_givenc,
+			  ctx->sh_desc_givenc_dma, edesc, req, contig);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "aead jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->hw_desc,
+		       desc_bytes(edesc->hw_desc), 1);
+#endif
+
 	desc = edesc->hw_desc;
+	ret = caam_jr_enqueue(jrdev, desc, aead_encrypt_done, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		aead_unmap(jrdev, edesc, req);
+		kfree(edesc);
+	}
 
-	/* insert shared descriptor pointer */
-	init_job_desc_shared(desc, ctx->shared_desc_phys,
-			     desc_len(ctx->sh_desc), HDR_SHARE_DEFER);
+	return ret;
+}
 
-	/*
-	 * LOAD IMM Info FIFO
-	 * to DECO, Last, Padding, Random, Message, 16 bytes
-	 */
-	append_load_imm_u32(desc, NFIFOENTRY_DEST_DECO | NFIFOENTRY_LC1 |
-			    NFIFOENTRY_STYPE_PAD | NFIFOENTRY_DTYPE_MSG |
-			    NFIFOENTRY_PTYPE_RND | ivsize,
-			    LDST_SRCDST_WORD_INFO_FIFO);
+/*
+ * allocate and map the ablkcipher extended descriptor for ablkcipher
+ */
+static struct ablkcipher_edesc *ablkcipher_edesc_alloc(struct ablkcipher_request
+						       *req, int desc_bytes,
+						       bool *iv_contig_out)
+{
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
+	struct device *jrdev = ctx->jrdev;
+	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
+					  CRYPTO_TFM_REQ_MAY_SLEEP)) ?
+		       GFP_KERNEL : GFP_ATOMIC;
+	int src_nents, dst_nents = 0, link_tbl_bytes;
+	struct ablkcipher_edesc *edesc;
+	dma_addr_t iv_dma = 0;
+	bool iv_contig = false;
+	int sgc;
+	int ivsize = crypto_ablkcipher_ivsize(ablkcipher);
+	int link_tbl_index;
+
+	src_nents = sg_count(req->src, req->nbytes);
+
+	if (unlikely(req->dst != req->src))
+		dst_nents = sg_count(req->dst, req->nbytes);
+
+	if (likely(req->src == req->dst)) {
+		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
+				 DMA_BIDIRECTIONAL);
+		dma_sync_sg_for_device(jrdev, req->src, sgc,
+				       DMA_BIDIRECTIONAL);
+	} else {
+		sgc = dma_map_sg(jrdev, req->src, src_nents ? : 1,
+				 DMA_TO_DEVICE);
+		dma_sync_sg_for_device(jrdev, req->src, sgc, DMA_TO_DEVICE);
+		sgc = dma_map_sg(jrdev, req->dst, dst_nents ? : 1,
+				 DMA_FROM_DEVICE);
+	}
 
+	/* FIXME: no test for sgc values returned above... */
 	/*
-	 * disable info fifo entries since the above serves as the entry
-	 * this way, the MOVE command won't generate an entry.
-	 * Note that this isn't required in more recent versions of
-	 * SEC as a MOVE that doesn't do info FIFO entries is available.
+	 * Check if iv can be contiguous with source and destination.
+	 * If so, include it. If not, create scatterlist.
 	 */
-	append_cmd(desc, CMD_LOAD | DISABLE_AUTO_INFO_FIFO);
+	iv_dma = dma_map_single(jrdev, req->info, ivsize, DMA_TO_DEVICE);
+	dma_sync_single_for_device(jrdev, iv_dma, ivsize, DMA_TO_DEVICE);
+	if (!src_nents && iv_dma + ivsize == sg_dma_address(req->src))
+		iv_contig = true;
+	else
+		src_nents = src_nents ? : 1;
+	link_tbl_bytes = ((iv_contig ? 0 : 1) + src_nents + dst_nents) *
+			 sizeof(struct link_tbl_entry);
 
-	/* MOVE DECO Alignment -> C1 Context 16 bytes */
-	append_move(desc, MOVE_SRC_INFIFO | MOVE_DEST_CLASS1CTX | ivsize);
+	/* allocate space for base edesc and hw desc commands, link tables */
+	edesc = kmalloc(sizeof(struct ablkcipher_edesc) + desc_bytes +
+			link_tbl_bytes, GFP_DMA | flags);
+	if (!edesc) {
+		dev_err(jrdev, "could not allocate extended descriptor\n");
+		return ERR_PTR(-ENOMEM);
+	}
 
-	/* re-enable info fifo entries */
-	append_cmd(desc, CMD_LOAD | ENABLE_AUTO_INFO_FIFO);
+	edesc->src_nents = src_nents;
+	edesc->dst_nents = dst_nents;
+	edesc->link_tbl_bytes = link_tbl_bytes;
+	edesc->link_tbl = (void *)edesc + sizeof(struct ablkcipher_edesc) +
+			  desc_bytes;
+
+	link_tbl_index = 0;
+	if (!iv_contig) {
+		sg_to_link_tbl_one(edesc->link_tbl, iv_dma, ivsize, 0);
+		sg_to_link_tbl_last(req->src, src_nents,
+				    edesc->link_tbl + 1, 0);
+		link_tbl_index += 1 + src_nents;
+	}
+
+	if (unlikely(dst_nents)) {
+		sg_to_link_tbl_last(req->dst, dst_nents,
+			edesc->link_tbl + link_tbl_index, 0);
+	}
+
+	edesc->link_tbl_dma = dma_map_single(jrdev, edesc->link_tbl,
+					     link_tbl_bytes, DMA_TO_DEVICE);
+	edesc->iv_dma = iv_dma;
 
-	/* MOVE C1 Context -> OFIFO 16 bytes */
-	append_move(desc, MOVE_SRC_CLASS1CTX | MOVE_DEST_OUTFIFO | ivsize);
+	dma_sync_single_for_device(jrdev, edesc->link_tbl_dma, link_tbl_bytes,
+				   DMA_TO_DEVICE);
 
-	append_fifo_store(desc, iv_dma, ivsize, FIFOST_TYPE_MESSAGE_DATA);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ablkcipher link_tbl@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->link_tbl,
+		       link_tbl_bytes, 1);
+#endif
 
-	return ipsec_esp(edesc, areq, OP_ALG_ENCRYPT, ipsec_esp_encrypt_done);
+	*iv_contig_out = iv_contig;
+	return edesc;
 }
 
+static int ablkcipher_encrypt(struct ablkcipher_request *req)
+{
+	struct ablkcipher_edesc *edesc;
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
+	struct device *jrdev = ctx->jrdev;
+	bool iv_contig;
+	u32 *desc;
+	int ret = 0;
+
+	/* allocate extended descriptor */
+	edesc = ablkcipher_edesc_alloc(req, DESC_JOB_IO_LEN *
+				       CAAM_CMD_SZ, &iv_contig);
+	if (IS_ERR(edesc))
+		return PTR_ERR(edesc);
+
+	/* Create and submit job descriptor*/
+	init_ablkcipher_job(ctx->sh_desc_enc,
+		ctx->sh_desc_enc_dma, edesc, req, iv_contig);
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ablkcipher jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->hw_desc,
+		       desc_bytes(edesc->hw_desc), 1);
+#endif
+	desc = edesc->hw_desc;
+	ret = caam_jr_enqueue(jrdev, desc, ablkcipher_encrypt_done, req);
+
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		ablkcipher_unmap(jrdev, edesc, req);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
+static int ablkcipher_decrypt(struct ablkcipher_request *req)
+{
+	struct ablkcipher_edesc *edesc;
+	struct crypto_ablkcipher *ablkcipher = crypto_ablkcipher_reqtfm(req);
+	struct caam_ctx *ctx = crypto_ablkcipher_ctx(ablkcipher);
+	struct device *jrdev = ctx->jrdev;
+	bool iv_contig;
+	u32 *desc;
+	int ret = 0;
+
+	/* allocate extended descriptor */
+	edesc = ablkcipher_edesc_alloc(req, DESC_JOB_IO_LEN *
+				       CAAM_CMD_SZ, &iv_contig);
+	if (IS_ERR(edesc))
+		return PTR_ERR(edesc);
+
+	/* Create and submit job descriptor*/
+	init_ablkcipher_job(ctx->sh_desc_dec,
+		ctx->sh_desc_dec_dma, edesc, req, iv_contig);
+	desc = edesc->hw_desc;
+#ifdef DEBUG
+	print_hex_dump(KERN_ERR, "ablkcipher jobdesc@"xstr(__LINE__)": ",
+		       DUMP_PREFIX_ADDRESS, 16, 4, edesc->hw_desc,
+		       desc_bytes(edesc->hw_desc), 1);
+#endif
+
+	ret = caam_jr_enqueue(jrdev, desc, ablkcipher_decrypt_done, req);
+	if (!ret) {
+		ret = -EINPROGRESS;
+	} else {
+		ablkcipher_unmap(jrdev, edesc, req);
+		kfree(edesc);
+	}
+
+	return ret;
+}
+
+#define template_aead		template_u.aead
+#define template_ablkcipher	template_u.ablkcipher
 struct caam_alg_template {
 	char name[CRYPTO_MAX_ALG_NAME];
 	char driver_name[CRYPTO_MAX_ALG_NAME];
 	unsigned int blocksize;
-	struct aead_alg aead;
+	u32 type;
+	union {
+		struct ablkcipher_alg ablkcipher;
+		struct aead_alg aead;
+		struct blkcipher_alg blkcipher;
+		struct cipher_alg cipher;
+		struct compress_alg compress;
+		struct rng_alg rng;
+	} template_u;
 	u32 class1_alg_type;
 	u32 class2_alg_type;
 	u32 alg_op;
@@ -900,12 +1879,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha1),cbc(aes))",
 		.driver_name = "authenc-hmac-sha1-cbc-aes-caam",
 		.blocksize = AES_BLOCK_SIZE,
-		.aead = {
-			.setkey = aead_authenc_setkey,
-			.setauthsize = aead_authenc_setauthsize,
-			.encrypt = aead_authenc_encrypt,
-			.decrypt = aead_authenc_decrypt,
-			.givencrypt = aead_authenc_givencrypt,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = AES_BLOCK_SIZE,
 			.maxauthsize = SHA1_DIGEST_SIZE,
@@ -918,12 +1898,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha256),cbc(aes))",
 		.driver_name = "authenc-hmac-sha256-cbc-aes-caam",
 		.blocksize = AES_BLOCK_SIZE,
-		.aead = {
-			.setkey = aead_authenc_setkey,
-			.setauthsize = aead_authenc_setauthsize,
-			.encrypt = aead_authenc_encrypt,
-			.decrypt = aead_authenc_decrypt,
-			.givencrypt = aead_authenc_givencrypt,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = AES_BLOCK_SIZE,
 			.maxauthsize = SHA256_DIGEST_SIZE,
@@ -937,12 +1918,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha512),cbc(aes))",
 		.driver_name = "authenc-hmac-sha512-cbc-aes-caam",
 		.blocksize = AES_BLOCK_SIZE,
-		.aead = {
-			.setkey = aead_authenc_setkey,
-			.setauthsize = aead_authenc_setauthsize,
-			.encrypt = aead_authenc_encrypt,
-			.decrypt = aead_authenc_decrypt,
-			.givencrypt = aead_authenc_givencrypt,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = AES_BLOCK_SIZE,
 			.maxauthsize = SHA512_DIGEST_SIZE,
@@ -956,12 +1938,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha1),cbc(des3_ede))",
 		.driver_name = "authenc-hmac-sha1-cbc-des3_ede-caam",
 		.blocksize = DES3_EDE_BLOCK_SIZE,
-		.aead = {
-			.setkey = aead_authenc_setkey,
-			.setauthsize = aead_authenc_setauthsize,
-			.encrypt = aead_authenc_encrypt,
-			.decrypt = aead_authenc_decrypt,
-			.givencrypt = aead_authenc_givencrypt,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES3_EDE_BLOCK_SIZE,
 			.maxauthsize = SHA1_DIGEST_SIZE,
@@ -974,12 +1957,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha256),cbc(des3_ede))",
 		.driver_name = "authenc-hmac-sha256-cbc-des3_ede-caam",
 		.blocksize = DES3_EDE_BLOCK_SIZE,
-		.aead = {
-			.setkey = aead_authenc_setkey,
-			.setauthsize = aead_authenc_setauthsize,
-			.encrypt = aead_authenc_encrypt,
-			.decrypt = aead_authenc_decrypt,
-			.givencrypt = aead_authenc_givencrypt,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES3_EDE_BLOCK_SIZE,
 			.maxauthsize = SHA256_DIGEST_SIZE,
@@ -993,12 +1977,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha512),cbc(des3_ede))",
 		.driver_name = "authenc-hmac-sha512-cbc-des3_ede-caam",
 		.blocksize = DES3_EDE_BLOCK_SIZE,
-		.aead = {
-			.setkey = aead_authenc_setkey,
-			.setauthsize = aead_authenc_setauthsize,
-			.encrypt = aead_authenc_encrypt,
-			.decrypt = aead_authenc_decrypt,
-			.givencrypt = aead_authenc_givencrypt,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES3_EDE_BLOCK_SIZE,
 			.maxauthsize = SHA512_DIGEST_SIZE,
@@ -1012,12 +1997,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha1),cbc(des))",
 		.driver_name = "authenc-hmac-sha1-cbc-des-caam",
 		.blocksize = DES_BLOCK_SIZE,
-		.aead = {
-			.setkey = aead_authenc_setkey,
-			.setauthsize = aead_authenc_setauthsize,
-			.encrypt = aead_authenc_encrypt,
-			.decrypt = aead_authenc_decrypt,
-			.givencrypt = aead_authenc_givencrypt,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES_BLOCK_SIZE,
 			.maxauthsize = SHA1_DIGEST_SIZE,
@@ -1030,12 +2016,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha256),cbc(des))",
 		.driver_name = "authenc-hmac-sha256-cbc-des-caam",
 		.blocksize = DES_BLOCK_SIZE,
-		.aead = {
-			.setkey = aead_authenc_setkey,
-			.setauthsize = aead_authenc_setauthsize,
-			.encrypt = aead_authenc_encrypt,
-			.decrypt = aead_authenc_decrypt,
-			.givencrypt = aead_authenc_givencrypt,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES_BLOCK_SIZE,
 			.maxauthsize = SHA256_DIGEST_SIZE,
@@ -1049,12 +2036,13 @@ static struct caam_alg_template driver_algs[] = {
 		.name = "authenc(hmac(sha512),cbc(des))",
 		.driver_name = "authenc-hmac-sha512-cbc-des-caam",
 		.blocksize = DES_BLOCK_SIZE,
-		.aead = {
-			.setkey = aead_authenc_setkey,
-			.setauthsize = aead_authenc_setauthsize,
-			.encrypt = aead_authenc_encrypt,
-			.decrypt = aead_authenc_decrypt,
-			.givencrypt = aead_authenc_givencrypt,
+		.type = CRYPTO_ALG_TYPE_AEAD,
+		.template_aead = {
+			.setkey = aead_setkey,
+			.setauthsize = aead_setauthsize,
+			.encrypt = aead_encrypt,
+			.decrypt = aead_decrypt,
+			.givencrypt = aead_givencrypt,
 			.geniv = "<built-in>",
 			.ivsize = DES_BLOCK_SIZE,
 			.maxauthsize = SHA512_DIGEST_SIZE,
@@ -1064,6 +2052,55 @@ static struct caam_alg_template driver_algs[] = {
 				   OP_ALG_AAI_HMAC_PRECOMP,
 		.alg_op = OP_ALG_ALGSEL_SHA512 | OP_ALG_AAI_HMAC,
 	},
+	/* ablkcipher descriptor */
+	{
+		.name = "cbc(aes)",
+		.driver_name = "cbc-aes-caam",
+		.blocksize = AES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ablkcipher_setkey,
+			.encrypt = ablkcipher_encrypt,
+			.decrypt = ablkcipher_decrypt,
+			.geniv = "eseqiv",
+			.min_keysize = AES_MIN_KEY_SIZE,
+			.max_keysize = AES_MAX_KEY_SIZE,
+			.ivsize = AES_BLOCK_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_AES | OP_ALG_AAI_CBC,
+	},
+	{
+		.name = "cbc(des3_ede)",
+		.driver_name = "cbc-3des-caam",
+		.blocksize = DES3_EDE_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ablkcipher_setkey,
+			.encrypt = ablkcipher_encrypt,
+			.decrypt = ablkcipher_decrypt,
+			.geniv = "eseqiv",
+			.min_keysize = DES3_EDE_KEY_SIZE,
+			.max_keysize = DES3_EDE_KEY_SIZE,
+			.ivsize = DES3_EDE_BLOCK_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_3DES | OP_ALG_AAI_CBC,
+	},
+	{
+		.name = "cbc(des)",
+		.driver_name = "cbc-des-caam",
+		.blocksize = DES_BLOCK_SIZE,
+		.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.template_ablkcipher = {
+			.setkey = ablkcipher_setkey,
+			.encrypt = ablkcipher_encrypt,
+			.decrypt = ablkcipher_decrypt,
+			.geniv = "eseqiv",
+			.min_keysize = DES_KEY_SIZE,
+			.max_keysize = DES_KEY_SIZE,
+			.ivsize = DES_BLOCK_SIZE,
+			},
+		.class1_alg_type = OP_ALG_ALGSEL_DES | OP_ALG_AAI_CBC,
+	}
 };
 
 struct caam_crypto_alg {
@@ -1102,38 +2139,29 @@ static void caam_cra_exit(struct crypto_tfm *tfm)
 {
 	struct caam_ctx *ctx = crypto_tfm_ctx(tfm);
 
-	if (!dma_mapping_error(ctx->jrdev, ctx->shared_desc_phys))
-		dma_unmap_single(ctx->jrdev, ctx->shared_desc_phys,
-				 desc_bytes(ctx->sh_desc), DMA_TO_DEVICE);
-	kfree(ctx->sh_desc);
-
-	if (!dma_mapping_error(ctx->jrdev, ctx->key_phys))
-		dma_unmap_single(ctx->jrdev, ctx->key_phys,
-				 ctx->split_key_pad_len + ctx->enckeylen,
+	if (ctx->sh_desc_enc_dma &&
+	    !dma_mapping_error(ctx->jrdev, ctx->sh_desc_enc_dma))
+		dma_unmap_single(ctx->jrdev, ctx->sh_desc_enc_dma,
+				 desc_bytes(ctx->sh_desc_enc), DMA_TO_DEVICE);
+	if (ctx->sh_desc_dec_dma &&
+	    !dma_mapping_error(ctx->jrdev, ctx->sh_desc_dec_dma))
+		dma_unmap_single(ctx->jrdev, ctx->sh_desc_dec_dma,
+				 desc_bytes(ctx->sh_desc_dec), DMA_TO_DEVICE);
+	if (ctx->sh_desc_givenc_dma &&
+	    !dma_mapping_error(ctx->jrdev, ctx->sh_desc_givenc_dma))
+		dma_unmap_single(ctx->jrdev, ctx->sh_desc_givenc_dma,
+				 desc_bytes(ctx->sh_desc_givenc),
 				 DMA_TO_DEVICE);
-	kfree(ctx->key);
 }
 
-static void __exit caam_algapi_exit(void)
+void caam_algapi_shutdown(struct platform_device *pdev)
 {
-
-	struct device_node *dev_node;
-	struct platform_device *pdev;
 	struct device *ctrldev;
 	struct caam_drv_private *priv;
 	struct caam_crypto_alg *t_alg, *n;
 	int i, err;
 
-	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
-	if (!dev_node)
-		return;
-
-	pdev = of_find_device_by_node(dev_node);
-	if (!pdev)
-		return;
-
 	ctrldev = &pdev->dev;
-	of_node_put(dev_node);
 	priv = dev_get_drvdata(ctrldev);
 
 	if (!priv->alg_list.next)
@@ -1175,12 +2203,20 @@ static struct caam_crypto_alg *caam_alg_alloc(struct device *ctrldev,
 	alg->cra_init = caam_cra_init;
 	alg->cra_exit = caam_cra_exit;
 	alg->cra_priority = CAAM_CRA_PRIORITY;
-	alg->cra_flags = CRYPTO_ALG_TYPE_AEAD | CRYPTO_ALG_ASYNC;
 	alg->cra_blocksize = template->blocksize;
 	alg->cra_alignmask = 0;
-	alg->cra_type = &crypto_aead_type;
 	alg->cra_ctxsize = sizeof(struct caam_ctx);
-	alg->cra_u.aead = template->aead;
+	alg->cra_flags = CRYPTO_ALG_ASYNC | template->type;
+	switch (template->type) {
+	case CRYPTO_ALG_TYPE_ABLKCIPHER:
+		alg->cra_type = &crypto_ablkcipher_type;
+		alg->cra_ablkcipher = template->template_ablkcipher;
+		break;
+	case CRYPTO_ALG_TYPE_AEAD:
+		alg->cra_type = &crypto_aead_type;
+		alg->cra_aead = template->template_aead;
+		break;
+	}
 
 	t_alg->class1_alg_type = template->class1_alg_type;
 	t_alg->class2_alg_type = template->class2_alg_type;
@@ -1190,26 +2226,14 @@ static struct caam_crypto_alg *caam_alg_alloc(struct device *ctrldev,
 	return t_alg;
 }
 
-static int __init caam_algapi_init(void)
+int caam_algapi_startup(struct platform_device *pdev)
 {
-	struct device_node *dev_node;
-	struct platform_device *pdev;
 	struct device *ctrldev, **jrdev;
 	struct caam_drv_private *priv;
 	int i = 0, err = 0;
 
-	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
-	if (!dev_node)
-		return -ENODEV;
-
-	pdev = of_find_device_by_node(dev_node);
-	if (!pdev)
-		return -ENODEV;
-
 	ctrldev = &pdev->dev;
 	priv = dev_get_drvdata(ctrldev);
-	of_node_put(dev_node);
-
 	INIT_LIST_HEAD(&priv->alg_list);
 
 	jrdev = kmalloc(sizeof(*jrdev) * priv->total_jobrs, GFP_KERNEL);
@@ -1260,9 +2284,51 @@ static int __init caam_algapi_init(void)
 	return err;
 }
 
+#ifdef CONFIG_OF
+static void __exit caam_algapi_exit(void)
+{
+
+	struct device_node *dev_node;
+	struct platform_device *pdev;
+
+	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
+	if (!dev_node)
+		return;
+
+	pdev = of_find_device_by_node(dev_node);
+	if (!pdev)
+		return;
+
+	caam_algapi_shutdown(pdev);
+
+	of_node_put(dev_node);
+}
+
+static int __init caam_algapi_init(void)
+{
+	struct device_node *dev_node;
+	struct platform_device *pdev;
+	int stat;
+
+	dev_node = of_find_compatible_node(NULL, NULL, "fsl,sec-v4.0");
+	if (!dev_node)
+		return -ENODEV;
+
+	pdev = of_find_device_by_node(dev_node);
+	if (!pdev)
+		return -ENODEV;
+
+	stat = caam_algapi_startup(pdev);
+
+	of_node_put(dev_node);
+
+	return stat;
+}
+
 module_init(caam_algapi_init);
 module_exit(caam_algapi_exit);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("FSL CAAM support for crypto API");
 MODULE_AUTHOR("Freescale Semiconductor - NMG/STC");
+#endif
diff --git a/drivers/crypto/caam/compat.h b/drivers/crypto/caam/compat.h
index 950450346f70..9ad93d4156c2 100644
--- a/drivers/crypto/caam/compat.h
+++ b/drivers/crypto/caam/compat.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2008-2012 Freescale Semiconductor, Inc.
  */
 
 #ifndef CAAM_COMPAT_H
@@ -12,7 +12,6 @@
 #include <linux/interrupt.h>
 #include <linux/crypto.h>
 #include <linux/hw_random.h>
-#include <linux/of_platform.h>
 #include <linux/dma-mapping.h>
 #include <linux/io.h>
 #include <linux/spinlock.h>
@@ -22,6 +21,18 @@
 #include <linux/types.h>
 #include <linux/debugfs.h>
 #include <linux/circ_buf.h>
+
+#ifdef CONFIG_OF
+#include <linux/of_platform.h>
+#else
+#include <linux/platform_device.h>
+#endif
+
+#ifdef CONFIG_ARM /* needs the clock control subsystem */
+#include <linux/clk.h>
+#include <asm/cacheflush.h>
+#endif
+
 #include <net/xfrm.h>
 
 #include <crypto/algapi.h>
@@ -31,5 +42,6 @@
 #include <crypto/aead.h>
 #include <crypto/authenc.h>
 #include <crypto/scatterwalk.h>
+#include <crypto/internal/skcipher.h>
 
 #endif /* !defined(CAAM_COMPAT_H) */
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index 9009713a3c2e..7c1132097c0f 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -2,7 +2,7 @@
  * CAAM control-plane driver backend
  * Controller-level driver, kernel property detection, initialization
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2008-2012 Freescale Semiconductor, Inc.
  */
 
 #include "compat.h"
@@ -22,6 +22,9 @@ static int caam_remove(struct platform_device *pdev)
 	ctrlpriv = dev_get_drvdata(ctrldev);
 	topregs = (struct caam_full __iomem *)ctrlpriv->ctrl;
 
+#ifndef CONFIG_OF
+	caam_algapi_shutdown(pdev);
+#endif
 	/* shut down JobRs */
 	for (ring = 0; ring < ctrlpriv->total_jobrs; ring++) {
 		ret |= caam_jr_shutdown(ctrlpriv->jrdev[ring]);
@@ -37,6 +40,9 @@ static int caam_remove(struct platform_device *pdev)
 	/* Unmap controller region */
 	iounmap(&topregs->ctrl);
 
+	/* shut clocks off before finalizing shutdown */
+	clk_disable(ctrlpriv->caam_clk);
+
 	kfree(ctrlpriv->jrdev);
 	kfree(ctrlpriv);
 
@@ -48,13 +54,24 @@ static int caam_probe(struct platform_device *pdev)
 {
 	int d, ring, rspec;
 	struct device *dev;
-	struct device_node *nprop, *np;
+	struct device_node *np;
 	struct caam_ctrl __iomem *ctrl;
 	struct caam_full __iomem *topregs;
 	struct caam_drv_private *ctrlpriv;
-	struct caam_perfmon *perfmon;
 	struct caam_deco **deco;
 	u32 deconum;
+#ifdef CONFIG_DEBUG_FS
+	struct caam_perfmon *perfmon;
+#endif
+#ifdef CONFIG_OF
+	struct device_node *nprop;
+#else
+	struct resource *res;
+	char *rname, inst;
+#endif
+#ifdef CONFIG_ARM
+	int ret = 0;
+#endif
 
 	ctrlpriv = kzalloc(sizeof(struct caam_drv_private), GFP_KERNEL);
 	if (!ctrlpriv)
@@ -63,22 +80,71 @@ static int caam_probe(struct platform_device *pdev)
 	dev = &pdev->dev;
 	dev_set_drvdata(dev, ctrlpriv);
 	ctrlpriv->pdev = pdev;
-	nprop = pdev->dev.of_node;
 
 	/* Get configuration properties from device tree */
 	/* First, get register page */
+#ifdef CONFIG_OF
+	nprop = pdev->dev.of_node;
 	ctrl = of_iomap(nprop, 0);
 	if (ctrl == NULL) {
 		dev_err(dev, "caam: of_iomap() failed\n");
 		return -ENOMEM;
 	}
+#else
+	/* Get the named resource for the controller base address */
+	res = platform_get_resource_byname(pdev,
+		IORESOURCE_MEM, "iobase_caam");
+	if (!res) {
+		dev_err(dev, "caam: invalid address resource type\n");
+		return -ENODEV;
+	}
+	ctrl = ioremap(res->start, SZ_64K);
+	if (ctrl == NULL) {
+		dev_err(dev, "caam: ioremap() failed\n");
+		return -ENOMEM;
+	}
+#endif
+
 	ctrlpriv->ctrl = (struct caam_ctrl __force *)ctrl;
 
 	/* topregs used to derive pointers to CAAM sub-blocks only */
 	topregs = (struct caam_full __iomem *)ctrl;
 
 	/* Get the IRQ of the controller (for security violations only) */
+#ifdef CONFIG_OF
 	ctrlpriv->secvio_irq = of_irq_to_resource(nprop, 0, NULL);
+#else
+	res = platform_get_resource_byname(pdev,
+		IORESOURCE_IRQ, "irq_sec_vio");
+	if (!res) {
+		dev_err(dev, "caam: invalid IRQ resource type\n");
+		return -ENODEV;
+	}
+	ctrlpriv->secvio_irq = res->start;
+#endif
+
+/*
+ * ARM targets tend to have clock control subsystems that can
+ * enable/disable clocking to our device. Turn clocking on to proceed
+ */
+#ifdef CONFIG_ARM
+	ctrlpriv->caam_clk = clk_get(&ctrlpriv->pdev->dev, "caam_clk");
+	if (IS_ERR(ctrlpriv->caam_clk)) {
+		ret = PTR_ERR(ctrlpriv->caam_clk);
+		dev_err(&ctrlpriv->pdev->dev,
+			"can't identify CAAM bus clk: %d\n", ret);
+		return -ENODEV;
+	}
+
+	ret = clk_enable(ctrlpriv->caam_clk);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "can't enable CAAM bus clock: %d\n", ret);
+		return -ENODEV;
+	}
+
+	pr_debug("%s caam_clk:%d\n", __func__,
+		(int)clk_get_rate(ctrlpriv->caam_clk));
+#endif
 
 	/*
 	 * Enable DECO watchdogs and, if this is a PHYS_ADDR_T_64BIT kernel,
@@ -87,8 +153,11 @@ static int caam_probe(struct platform_device *pdev)
 	setbits32(&topregs->ctrl.mcr, MCFGR_WDENABLE |
 		  (sizeof(dma_addr_t) == sizeof(u64) ? MCFGR_LONG_PTR : 0));
 
+	/* Set DMA masks according to platform ranging */
 	if (sizeof(dma_addr_t) == sizeof(u64))
 		dma_set_mask(dev, DMA_BIT_MASK(36));
+	else
+		dma_set_mask(dev, DMA_BIT_MASK(32));
 
 	/* Find out how many DECOs are present */
 	deconum = (rd_reg64(&topregs->ctrl.perfmon.cha_num) &
@@ -100,15 +169,41 @@ static int caam_probe(struct platform_device *pdev)
 	deco = (struct caam_deco __force **)&topregs->deco;
 	for (d = 0; d < deconum; d++)
 		ctrlpriv->deco[d] = deco[d];
-
 	/*
 	 * Detect and enable JobRs
 	 * First, find out how many ring spec'ed, allocate references
 	 * for all, then go probe each one.
 	 */
 	rspec = 0;
+#ifdef CONFIG_OF
 	for_each_compatible_node(np, NULL, "fsl,sec-v4.0-job-ring")
 		rspec++;
+#else
+	np = NULL;
+
+	/* Build the name of the IRQ platform resources to identify */
+	rname = kzalloc(strlen(JR_IRQRES_NAME_ROOT) + 1, 0);
+	if (rname == NULL) {
+		iounmap(&topregs->ctrl);
+		return -ENOMEM;
+	}
+
+	/*
+	 * Emulate behavor of for_each_compatible_node() for non OF targets
+	 * Identify all IRQ platform resources present
+	 */
+	for (d = 0; d < 4; d++)	{
+		rname[0] = 0;
+		inst = '0' + d;
+		strcat(rname, JR_IRQRES_NAME_ROOT);
+		strncat(rname, &inst, 1);
+		res = platform_get_resource_byname(pdev,
+					IORESOURCE_IRQ, rname);
+		if (res)
+			rspec++;
+	}
+	kfree(rname);
+#endif
 	ctrlpriv->jrdev = kzalloc(sizeof(struct device *) * rspec, GFP_KERNEL);
 	if (ctrlpriv->jrdev == NULL) {
 		iounmap(&topregs->ctrl);
@@ -117,7 +212,11 @@ static int caam_probe(struct platform_device *pdev)
 
 	ring = 0;
 	ctrlpriv->total_jobrs = 0;
+#ifdef CONFIG_OF
 	for_each_compatible_node(np, NULL, "fsl,sec-v4.0-job-ring") {
+#else
+	for (d = 0; d < rspec; d++)	{
+#endif
 		caam_jr_probe(pdev, np, ring);
 		ctrlpriv->total_jobrs++;
 		ring++;
@@ -164,52 +263,52 @@ static int caam_probe(struct platform_device *pdev)
 	/* Controller-level - performance monitor counters */
 	ctrlpriv->ctl_rq_dequeued =
 		debugfs_create_u64("rq_dequeued",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->req_dequeued);
 	ctrlpriv->ctl_ob_enc_req =
 		debugfs_create_u64("ob_rq_encrypted",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->ob_enc_req);
 	ctrlpriv->ctl_ib_dec_req =
 		debugfs_create_u64("ib_rq_decrypted",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->ib_dec_req);
 	ctrlpriv->ctl_ob_enc_bytes =
 		debugfs_create_u64("ob_bytes_encrypted",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->ob_enc_bytes);
 	ctrlpriv->ctl_ob_prot_bytes =
 		debugfs_create_u64("ob_bytes_protected",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->ob_prot_bytes);
 	ctrlpriv->ctl_ib_dec_bytes =
 		debugfs_create_u64("ib_bytes_decrypted",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->ib_dec_bytes);
 	ctrlpriv->ctl_ib_valid_bytes =
 		debugfs_create_u64("ib_bytes_validated",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->ib_valid_bytes);
 
 	/* Controller level - global status values */
 	ctrlpriv->ctl_faultaddr =
 		debugfs_create_u64("fault_addr",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->faultaddr);
 	ctrlpriv->ctl_faultdetail =
 		debugfs_create_u32("fault_detail",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->faultdetail);
 	ctrlpriv->ctl_faultstatus =
 		debugfs_create_u32("fault_status",
-				   S_IFCHR | S_IRUSR | S_IRGRP | S_IROTH,
+				   S_IRUSR | S_IRGRP | S_IROTH,
 				   ctrlpriv->ctl, &perfmon->status);
 
 	/* Internal covering keys (useful in non-secure mode only) */
 	ctrlpriv->ctl_kek_wrap.data = &ctrlpriv->ctrl->kek[0];
 	ctrlpriv->ctl_kek_wrap.size = KEK_KEY_SIZE * sizeof(u32);
 	ctrlpriv->ctl_kek = debugfs_create_blob("kek",
-						S_IFCHR | S_IRUSR |
+						S_IRUSR |
 						S_IRGRP | S_IROTH,
 						ctrlpriv->ctl,
 						&ctrlpriv->ctl_kek_wrap);
@@ -217,7 +316,7 @@ static int caam_probe(struct platform_device *pdev)
 	ctrlpriv->ctl_tkek_wrap.data = &ctrlpriv->ctrl->tkek[0];
 	ctrlpriv->ctl_tkek_wrap.size = KEK_KEY_SIZE * sizeof(u32);
 	ctrlpriv->ctl_tkek = debugfs_create_blob("tkek",
-						 S_IFCHR | S_IRUSR |
+						 S_IRUSR |
 						 S_IRGRP | S_IROTH,
 						 ctrlpriv->ctl,
 						 &ctrlpriv->ctl_tkek_wrap);
@@ -225,14 +324,27 @@ static int caam_probe(struct platform_device *pdev)
 	ctrlpriv->ctl_tdsk_wrap.data = &ctrlpriv->ctrl->tdsk[0];
 	ctrlpriv->ctl_tdsk_wrap.size = KEK_KEY_SIZE * sizeof(u32);
 	ctrlpriv->ctl_tdsk = debugfs_create_blob("tdsk",
-						 S_IFCHR | S_IRUSR |
+						 S_IRUSR |
 						 S_IRGRP | S_IROTH,
 						 ctrlpriv->ctl,
 						 &ctrlpriv->ctl_tdsk_wrap);
 #endif
+
+/*
+ * Non OF configurations use plaform_device, and therefore cannot simply
+ * go and get a device node by name, which the algapi module startup code
+ * assumes is possible. Therefore, non OF configurations will have to
+ * start up the API code explicitly, and forego modularization
+ */
+#ifndef CONFIG_OF
+	/* FIXME: check status */
+	caam_algapi_startup(pdev);
+#endif
+
 	return 0;
 }
 
+#ifdef CONFIG_OF
 static struct of_device_id caam_match[] = {
 	{
 		.compatible = "fsl,sec-v4.0",
@@ -240,12 +352,17 @@ static struct of_device_id caam_match[] = {
 	{},
 };
 MODULE_DEVICE_TABLE(of, caam_match);
+#endif /* CONFIG_OF */
 
 static struct platform_driver caam_driver = {
 	.driver = {
 		.name = "caam",
 		.owner = THIS_MODULE,
+#ifdef CONFIG_OF
 		.of_match_table = caam_match,
+#else
+
+#endif
 	},
 	.probe       = caam_probe,
 	.remove      = __devexit_p(caam_remove),
@@ -253,12 +370,20 @@ static struct platform_driver caam_driver = {
 
 static int __init caam_base_init(void)
 {
+#ifdef CONFIG_OF
+	return of_register_platform_driver(&caam_driver);
+#else
 	return platform_driver_register(&caam_driver);
+#endif
 }
 
 static void __exit caam_base_exit(void)
 {
+#ifdef CONFIG_OF
+	return of_unregister_platform_driver(&caam_driver);
+#else
 	return platform_driver_unregister(&caam_driver);
+#endif
 }
 
 module_init(caam_base_init);
diff --git a/drivers/crypto/caam/desc_constr.h b/drivers/crypto/caam/desc_constr.h
index 46915800c26f..1e5d54c72aa8 100644
--- a/drivers/crypto/caam/desc_constr.h
+++ b/drivers/crypto/caam/desc_constr.h
@@ -1,7 +1,7 @@
 /*
  * caam descriptor construction helper functions
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2008-2012 Freescale Semiconductor, Inc.
  */
 
 #include "desc.h"
@@ -9,7 +9,7 @@
 #define IMMEDIATE (1 << 23)
 #define CAAM_CMD_SZ sizeof(u32)
 #define CAAM_PTR_SZ sizeof(dma_addr_t)
-#define CAAM_DESC_BYTES_MAX (CAAM_CMD_SZ * 64)
+#define CAAM_DESC_BYTES_MAX (CAAM_CMD_SZ * MAX_CAAM_DESCSIZE)
 
 #ifdef DEBUG
 #define PRINT_POS do { printk(KERN_DEBUG "%02d: %s\n", desc_len(desc),\
@@ -18,6 +18,9 @@
 #define PRINT_POS
 #endif
 
+#define SET_OK_PROP_ERRORS (IMMEDIATE | LDST_CLASS_DECO | \
+			    LDST_SRCDST_WORD_DECOCTRL | \
+			    (LDOFF_CHG_SHARE_OK_PROP << LDST_OFFSET_SHIFT))
 #define DISABLE_AUTO_INFO_FIFO (IMMEDIATE | LDST_CLASS_DECO | \
 				LDST_SRCDST_WORD_DECOCTRL | \
 				(LDOFF_DISABLE_AUTO_NFIFO << LDST_OFFSET_SHIFT))
@@ -203,3 +206,56 @@ static inline void append_##cmd##_imm_##type(u32 *desc, type immediate, \
 	append_cmd(desc, immediate); \
 }
 APPEND_CMD_RAW_IMM(load, LOAD, u32);
+
+/*
+ * Append math command. Only the last part of destination and source need to
+ * be specified
+ */
+#define APPEND_MATH(op, desc, dest, src_0, src_1, len) \
+append_cmd(desc, CMD_MATH | MATH_FUN_##op | MATH_DEST_##dest | \
+	   MATH_SRC0_##src_0 | MATH_SRC1_##src_1 | (u32) (len & MATH_LEN_MASK));
+
+#define append_math_add(desc, dest, src0, src1, len) \
+	APPEND_MATH(ADD, desc, dest, src0, src1, len)
+#define append_math_sub(desc, dest, src0, src1, len) \
+	APPEND_MATH(SUB, desc, dest, src0, src1, len)
+#define append_math_add_c(desc, dest, src0, src1, len) \
+	APPEND_MATH(ADDC, desc, dest, src0, src1, len)
+#define append_math_sub_b(desc, dest, src0, src1, len) \
+	APPEND_MATH(SUBB, desc, dest, src0, src1, len)
+#define append_math_and(desc, dest, src0, src1, len) \
+	APPEND_MATH(AND, desc, dest, src0, src1, len)
+#define append_math_or(desc, dest, src0, src1, len) \
+	APPEND_MATH(OR, desc, dest, src0, src1, len)
+#define append_math_xor(desc, dest, src0, src1, len) \
+	APPEND_MATH(XOR, desc, dest, src0, src1, len)
+#define append_math_lshift(desc, dest, src0, src1, len) \
+	APPEND_MATH(LSHIFT, desc, dest, src0, src1, len)
+#define append_math_rshift(desc, dest, src0, src1, len) \
+	APPEND_MATH(RSHIFT, desc, dest, src0, src1, len)
+
+/* Exactly one source is IMM. Data is passed in as u32 value */
+#define APPEND_MATH_IMM_u32(op, desc, dest, src_0, src_1, data) \
+do { \
+	APPEND_MATH(op, desc, dest, src_0, src_1, CAAM_CMD_SZ); \
+	append_cmd(desc, data); \
+} while (0);
+
+#define append_math_add_imm_u32(desc, dest, src0, src1, data) \
+	APPEND_MATH_IMM_u32(ADD, desc, dest, src0, src1, data)
+#define append_math_sub_imm_u32(desc, dest, src0, src1, data) \
+	APPEND_MATH_IMM_u32(SUB, desc, dest, src0, src1, data)
+#define append_math_add_c_imm_u32(desc, dest, src0, src1, data) \
+	APPEND_MATH_IMM_u32(ADDC, desc, dest, src0, src1, data)
+#define append_math_sub_b_imm_u32(desc, dest, src0, src1, data) \
+	APPEND_MATH_IMM_u32(SUBB, desc, dest, src0, src1, data)
+#define append_math_and_imm_u32(desc, dest, src0, src1, data) \
+	APPEND_MATH_IMM_u32(AND, desc, dest, src0, src1, data)
+#define append_math_or_imm_u32(desc, dest, src0, src1, data) \
+	APPEND_MATH_IMM_u32(OR, desc, dest, src0, src1, data)
+#define append_math_xor_imm_u32(desc, dest, src0, src1, data) \
+	APPEND_MATH_IMM_u32(XOR, desc, dest, src0, src1, data)
+#define append_math_lshift_imm_u32(desc, dest, src0, src1, data) \
+	APPEND_MATH_IMM_u32(LSHIFT, desc, dest, src0, src1, data)
+#define append_math_rshift_imm_u32(desc, dest, src0, src1, data) \
+	APPEND_MATH_IMM_u32(RSHIFT, desc, dest, src0, src1, data)
diff --git a/drivers/crypto/caam/intern.h b/drivers/crypto/caam/intern.h
index a34be01b0b29..19e4f25c25a8 100644
--- a/drivers/crypto/caam/intern.h
+++ b/drivers/crypto/caam/intern.h
@@ -2,7 +2,7 @@
  * CAAM/SEC 4.x driver backend
  * Private/internal definitions between modules
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2008-2012 Freescale Semiconductor, Inc.
  *
  */
 
@@ -26,6 +26,23 @@
 #define JOBR_INTC_COUNT_THLD 0
 #endif
 
+#ifndef CONFIG_OF
+#define JR_IRQRES_NAME_ROOT "irq_jr"
+#define JR_MEMRES_NAME_ROOT "offset_jr"
+#endif
+
+#ifdef CONFIG_ARM
+/*
+ * FIXME: ARM tree doesn't seem to provide this, ergo it seems to be
+ * in "platform limbo". Find a better place, perhaps.
+ */
+static inline void irq_dispose_mapping(unsigned int virq)
+{
+	return;
+}
+#endif
+
+
 /*
  * Storage for tracking each in-process entry moving across a ring
  * Each entry on an output ring needs one of these
@@ -91,6 +108,10 @@ struct caam_drv_private {
 	/* list of registered crypto algorithms (mk generic context handle?) */
 	struct list_head alg_list;
 
+#ifdef CONFIG_ARM
+	struct clk *caam_clk;
+#endif
+
 	/*
 	 * debugfs entries for developer view into driver/device
 	 * variables at runtime.
@@ -108,6 +129,7 @@ struct caam_drv_private {
 #endif
 };
 
-void caam_jr_algapi_init(struct device *dev);
-void caam_jr_algapi_remove(struct device *dev);
+void caam_algapi_shutdown(struct platform_device *pdev);
+int caam_algapi_startup(struct platform_device *pdev);
+
 #endif /* INTERN_H */
diff --git a/drivers/crypto/caam/jr.c b/drivers/crypto/caam/jr.c
index 340fa322c0f0..280aaaa3d4d4 100644
--- a/drivers/crypto/caam/jr.c
+++ b/drivers/crypto/caam/jr.c
@@ -2,7 +2,7 @@
  * CAAM/SEC 4.x transport/backend driver
  * JobR backend functionality
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright 2008-2012 Freescale Semiconductor, Inc.
  */
 
 #include "compat.h"
@@ -57,9 +57,15 @@ static void caam_jr_dequeue(unsigned long devarg)
 	struct caam_drv_private_jr *jrp = dev_get_drvdata(dev);
 	void (*usercall)(struct device *dev, u32 *desc, u32 status, void *arg);
 	u32 *userdesc, userstatus;
+	dma_addr_t outbusaddr;
 	void *userarg;
 	unsigned long flags;
 
+	outbusaddr = rd_reg64(&jrp->rregs->outring_base);
+	dma_sync_single_for_cpu(dev, outbusaddr,
+				sizeof(struct jr_outentry) * JOBR_DEPTH,
+				DMA_FROM_DEVICE);
+
 	spin_lock_irqsave(&jrp->outlock, flags);
 
 	head = ACCESS_ONCE(jrp->head);
@@ -73,7 +79,6 @@ static void caam_jr_dequeue(unsigned long devarg)
 			sw_idx = (tail + i) & (JOBR_DEPTH - 1);
 
 			smp_read_barrier_depends();
-
 			if (jrp->outring[hw_idx].desc ==
 			    jrp->entinfo[sw_idx].desc_addr_dma)
 				break; /* found */
@@ -240,7 +245,7 @@ int caam_jr_enqueue(struct device *dev, u32 *desc,
 	struct caam_jrentry_info *head_entry;
 	unsigned long flags;
 	int head, tail, desc_size;
-	dma_addr_t desc_dma;
+	dma_addr_t desc_dma, inpbusaddr;
 
 	desc_size = (*desc & HDR_JD_LENGTH_MASK) * sizeof(u32);
 	desc_dma = dma_map_single(dev, desc, desc_size, DMA_TO_DEVICE);
@@ -249,6 +254,12 @@ int caam_jr_enqueue(struct device *dev, u32 *desc,
 		return -EIO;
 	}
 
+	dma_sync_single_for_device(dev, desc_dma, desc_size, DMA_TO_DEVICE);
+
+	inpbusaddr = rd_reg64(&jrp->rregs->inpring_base);
+	dma_sync_single_for_device(dev, inpbusaddr,
+					sizeof(dma_addr_t) * JOBR_DEPTH,
+					DMA_TO_DEVICE);
 	spin_lock_irqsave(&jrp->inplock, flags);
 
 	head = jrp->head;
@@ -270,6 +281,10 @@ int caam_jr_enqueue(struct device *dev, u32 *desc,
 
 	jrp->inpring[jrp->inp_ring_write_index] = desc_dma;
 
+	dma_sync_single_for_device(dev, inpbusaddr,
+					sizeof(dma_addr_t) * JOBR_DEPTH,
+					DMA_TO_DEVICE);
+
 	smp_wmb();
 
 	jrp->inp_ring_write_index = (jrp->inp_ring_write_index + 1) &
@@ -343,7 +358,7 @@ static int caam_jr_init(struct device *dev)
 			     (unsigned long)dev);
 
 	error = request_irq(jrp->irq, caam_jr_interrupt, IRQF_SHARED,
-			    "caam-jobr", dev);
+			    "caam-jr", dev);
 	if (error) {
 		dev_err(dev, "can't connect JobR %d interrupt (%d)\n",
 			jrp->ridx, jrp->irq);
@@ -377,7 +392,7 @@ static int caam_jr_init(struct device *dev)
 	/* Setup rings */
 	inpbusaddr = dma_map_single(dev, jrp->inpring,
 				    sizeof(u32 *) * JOBR_DEPTH,
-				    DMA_BIDIRECTIONAL);
+				    DMA_TO_DEVICE);
 	if (dma_mapping_error(dev, inpbusaddr)) {
 		dev_err(dev, "caam_jr_init(): can't map input ring\n");
 		kfree(jrp->inpring);
@@ -388,12 +403,12 @@ static int caam_jr_init(struct device *dev)
 
 	outbusaddr = dma_map_single(dev, jrp->outring,
 				    sizeof(struct jr_outentry) * JOBR_DEPTH,
-				    DMA_BIDIRECTIONAL);
+				    DMA_FROM_DEVICE);
 	if (dma_mapping_error(dev, outbusaddr)) {
 		dev_err(dev, "caam_jr_init(): can't map output ring\n");
 			dma_unmap_single(dev, inpbusaddr,
 					 sizeof(u32 *) * JOBR_DEPTH,
-					 DMA_BIDIRECTIONAL);
+					 DMA_TO_DEVICE);
 		kfree(jrp->inpring);
 		kfree(jrp->outring);
 		kfree(jrp->entinfo);
@@ -446,11 +461,9 @@ int caam_jr_shutdown(struct device *dev)
 	outbusaddr = rd_reg64(&jrp->rregs->outring_base);
 	dma_unmap_single(dev, outbusaddr,
 			 sizeof(struct jr_outentry) * JOBR_DEPTH,
-			 DMA_BIDIRECTIONAL);
+			 DMA_FROM_DEVICE);
 	dma_unmap_single(dev, inpbusaddr, sizeof(u32 *) * JOBR_DEPTH,
-			 DMA_BIDIRECTIONAL);
-	kfree(jrp->outring);
-	kfree(jrp->inpring);
+			 DMA_TO_DEVICE);
 	kfree(jrp->entinfo);
 
 	return ret;
@@ -467,8 +480,12 @@ int caam_jr_probe(struct platform_device *pdev, struct device_node *np,
 	struct platform_device *jr_pdev;
 	struct caam_drv_private *ctrlpriv;
 	struct caam_drv_private_jr *jrpriv;
-	u32 *jroffset;
 	int error;
+	/* FIXME: perhaps "struct resource *" for OF and non? */
+	u32 *jroffset, *irqres;
+#ifndef CONFIG_OF
+	char *rname, rinst;
+#endif
 
 	ctrldev = &pdev->dev;
 	ctrlpriv = dev_get_drvdata(ctrldev);
@@ -489,12 +506,35 @@ int caam_jr_probe(struct platform_device *pdev, struct device_node *np,
 	 * need to add in the offset to this JobR. Don't know if I
 	 * like this long-term, but it'll run
 	 */
+#ifdef CONFIG_OF
 	jroffset = (u32 *)of_get_property(np, "reg", NULL);
+#else
+	rname = kmalloc(strlen(JR_MEMRES_NAME_ROOT) + 1, 0);
+	if (rname == NULL) {
+		dev_err(ctrldev, "can't alloc resource detection buffer %d\n",
+			ring);
+		kfree(jrpriv);
+		return -ENOMEM;
+	}
+	rname[0] = 0;
+	rinst = '0' + ring;
+	strcat(rname, JR_MEMRES_NAME_ROOT);
+	strncat(rname, &rinst, 1);
+	jroffset = (u32 *)platform_get_resource_byname(pdev, IORESOURCE_MEM,
+						       rname);
+	kfree(rname);
+#endif
 	jrpriv->rregs = (struct caam_job_ring __iomem *)((void *)ctrlpriv->ctrl
 							 + *jroffset);
 
 	/* Build a local dev for each detected queue */
+#ifdef CONFIG_OF
 	jr_pdev = of_platform_device_create(np, NULL, ctrldev);
+#else
+	jr_pdev = platform_device_register_data(ctrldev, "caam_jr", ring,
+						jrpriv,
+					sizeof(struct caam_drv_private_jr));
+#endif
 	if (jr_pdev == NULL) {
 		kfree(jrpriv);
 		return -EINVAL;
@@ -504,7 +544,24 @@ int caam_jr_probe(struct platform_device *pdev, struct device_node *np,
 	ctrlpriv->jrdev[ring] = jrdev;
 
 	/* Identify the interrupt */
+#ifdef CONFIG_OF
 	jrpriv->irq = of_irq_to_resource(np, 0, NULL);
+#else
+	rname = kmalloc(strlen(JR_IRQRES_NAME_ROOT) + 1, 0);
+	if (rname == NULL) {
+		dev_err(ctrldev, "can't alloc resource detection buffer %d\n",
+			ring);
+		kfree(jrpriv);
+		return -ENOMEM;
+	}
+	rname[0] = 0;
+	strcat(rname, JR_IRQRES_NAME_ROOT);
+	strncat(rname, &rinst, 1);
+	irqres = (u32 *)platform_get_resource_byname(pdev, IORESOURCE_IRQ,
+						     rname);
+	jrpriv->irq = *irqres;
+	kfree(rname);
+#endif
 
 	/* Now do the platform independent part */
 	error = caam_jr_init(jrdev); /* now turn on hardware */
diff --git a/drivers/crypto/caam/regs.h b/drivers/crypto/caam/regs.h
index aee394e39056..888706d2801c 100644
--- a/drivers/crypto/caam/regs.h
+++ b/drivers/crypto/caam/regs.h
@@ -1,7 +1,7 @@
 /*
  * CAAM hardware register-level view
  *
- * Copyright 2008-2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2008-2012 Freescale Semiconductor, Inc.
  */
 
 #ifndef REGS_H
@@ -74,15 +74,21 @@
 #endif
 #else
 #ifdef __LITTLE_ENDIAN
-#define wr_reg32(reg, data) __raw_writel(reg, data)
-#define rd_reg32(reg) __raw_readl(reg)
+#define wr_reg32(reg, data) writel(data, reg)
+#define rd_reg32(reg) readl(reg)
 #ifdef CONFIG_64BIT
-#define wr_reg64(reg, data) __raw_writeq(reg, data)
-#define rd_reg64(reg) __raw_readq(reg)
+#define wr_reg64(reg, data) writeq(data, reg)
+#define rd_reg64(reg) readq(reg)
 #endif
 #endif
 #endif
 
+#ifdef CONFIG_ARM
+/* These are common macros for Power, put here for ARMs */
+#define setbits32(_addr, _v) writel((readl(_addr) | (_v)), (_addr))
+#define clrbits32(_addr, _v) writel((readl(_addr) & ~(_v)), (_addr))
+#endif
+
 #ifndef CONFIG_64BIT
 static inline void wr_reg64(u64 __iomem *reg, u64 data)
 {
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index 306dcdd0dc38..a4ed05342168 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -340,9 +340,9 @@ static int sdma_config_ownership(struct sdma_channel *sdmac,
 	if (event_override && mcu_override && dsp_override)
 		return -EINVAL;
 
-	evt = __raw_readl(sdma->regs + SDMA_H_EVTOVR);
-	mcu = __raw_readl(sdma->regs + SDMA_H_HOSTOVR);
-	dsp = __raw_readl(sdma->regs + SDMA_H_DSPOVR);
+	evt = readl_relaxed(sdma->regs + SDMA_H_EVTOVR);
+	mcu = readl_relaxed(sdma->regs + SDMA_H_HOSTOVR);
+	dsp = readl_relaxed(sdma->regs + SDMA_H_DSPOVR);
 
 	if (dsp_override)
 		dsp &= ~(1 << channel);
@@ -359,9 +359,9 @@ static int sdma_config_ownership(struct sdma_channel *sdmac,
 	else
 		mcu |= (1 << channel);
 
-	__raw_writel(evt, sdma->regs + SDMA_H_EVTOVR);
-	__raw_writel(mcu, sdma->regs + SDMA_H_HOSTOVR);
-	__raw_writel(dsp, sdma->regs + SDMA_H_DSPOVR);
+	writel_relaxed(evt, sdma->regs + SDMA_H_EVTOVR);
+	writel_relaxed(mcu, sdma->regs + SDMA_H_HOSTOVR);
+	writel_relaxed(dsp, sdma->regs + SDMA_H_DSPOVR);
 
 	return 0;
 }
@@ -378,7 +378,7 @@ static int sdma_run_channel(struct sdma_channel *sdmac)
 	init_completion(&sdmac->done);
 
 	wmb();
-	__raw_writel(1 << channel, sdma->regs + SDMA_H_START);
+	writel_relaxed(1 << channel, sdma->regs + SDMA_H_START);
 
 	ret = wait_for_completion_timeout(&sdmac->done, HZ);
 
@@ -421,9 +421,9 @@ static void sdma_event_enable(struct sdma_channel *sdmac, unsigned int event)
 	u32 val;
 	u32 chnenbl = chnenbl_ofs(sdma, event);
 
-	val = __raw_readl(sdma->regs + chnenbl);
+	val = readl_relaxed(sdma->regs + chnenbl);
 	val |= (1 << channel);
-	__raw_writel(val, sdma->regs + chnenbl);
+	writel_relaxed(val, sdma->regs + chnenbl);
 }
 
 static void sdma_event_disable(struct sdma_channel *sdmac, unsigned int event)
@@ -433,9 +433,9 @@ static void sdma_event_disable(struct sdma_channel *sdmac, unsigned int event)
 	u32 chnenbl = chnenbl_ofs(sdma, event);
 	u32 val;
 
-	val = __raw_readl(sdma->regs + chnenbl);
+	val = readl_relaxed(sdma->regs + chnenbl);
 	val &= ~(1 << channel);
-	__raw_writel(val, sdma->regs + chnenbl);
+	writel_relaxed(val, sdma->regs + chnenbl);
 }
 
 static void sdma_handle_channel_loop(struct sdma_channel *sdmac)
@@ -512,8 +512,8 @@ static irqreturn_t sdma_int_handler(int irq, void *dev_id)
 	struct sdma_engine *sdma = dev_id;
 	u32 stat;
 
-	stat = __raw_readl(sdma->regs + SDMA_H_INTR);
-	__raw_writel(stat, sdma->regs + SDMA_H_INTR);
+	stat = readl_relaxed(sdma->regs + SDMA_H_INTR);
+	writel_relaxed(stat, sdma->regs + SDMA_H_INTR);
 
 	while (stat) {
 		int channel = fls(stat) - 1;
@@ -673,7 +673,7 @@ static void sdma_disable_channel(struct sdma_channel *sdmac)
 	struct sdma_engine *sdma = sdmac->sdma;
 	int channel = sdmac->channel;
 
-	__raw_writel(1 << channel, sdma->regs + SDMA_H_STATSTOP);
+	writel_relaxed(1 << channel, sdma->regs + SDMA_H_STATSTOP);
 	sdmac->status = DMA_ERROR;
 }
 
@@ -778,7 +778,7 @@ static int sdma_set_channel_priority(struct sdma_channel *sdmac,
 		return -EINVAL;
 	}
 
-	__raw_writel(priority, sdma->regs + SDMA_CHNPRI_0 + 4 * channel);
+	writel_relaxed(priority, sdma->regs + SDMA_CHNPRI_0 + 4 * channel);
 
 	return 0;
 }
@@ -817,7 +817,7 @@ out:
 static void sdma_enable_channel(struct sdma_engine *sdma, int channel)
 {
 	wmb();
-	__raw_writel(1 << channel, sdma->regs + SDMA_H_START);
+	writel(1 << channel, sdma->regs + SDMA_H_START);
 }
 
 static dma_cookie_t sdma_assign_cookie(struct sdma_channel *sdmac)
@@ -1246,7 +1246,7 @@ static int __init sdma_init(struct sdma_engine *sdma)
 	clk_enable(sdma->clk);
 
 	/* Be sure SDMA has not started yet */
-	__raw_writel(0, sdma->regs + SDMA_H_C0PTR);
+	writel_relaxed(0, sdma->regs + SDMA_H_C0PTR);
 
 	sdma->channel_control = dma_alloc_coherent(NULL,
 			MAX_DMA_CHANNELS * sizeof (struct sdma_channel_control) +
@@ -1269,11 +1269,11 @@ static int __init sdma_init(struct sdma_engine *sdma)
 
 	/* disable all channels */
 	for (i = 0; i < sdma->num_events; i++)
-		__raw_writel(0, sdma->regs + chnenbl_ofs(sdma, i));
+		writel_relaxed(0, sdma->regs + chnenbl_ofs(sdma, i));
 
 	/* All channels have priority 0 */
 	for (i = 0; i < MAX_DMA_CHANNELS; i++)
-		__raw_writel(0, sdma->regs + SDMA_CHNPRI_0 + i * 4);
+		writel_relaxed(0, sdma->regs + SDMA_CHNPRI_0 + i * 4);
 
 	ret = sdma_request_channel(&sdma->channel[0]);
 	if (ret)
@@ -1282,16 +1282,16 @@ static int __init sdma_init(struct sdma_engine *sdma)
 	sdma_config_ownership(&sdma->channel[0], false, true, false);
 
 	/* Set Command Channel (Channel Zero) */
-	__raw_writel(0x4050, sdma->regs + SDMA_CHN0ADDR);
+	writel_relaxed(0x4050, sdma->regs + SDMA_CHN0ADDR);
 
 	/* Set bits of CONFIG register but with static context switching */
 	/* FIXME: Check whether to set ACR bit depending on clock ratios */
-	__raw_writel(0, sdma->regs + SDMA_H_CONFIG);
+	writel_relaxed(0, sdma->regs + SDMA_H_CONFIG);
 
-	__raw_writel(ccb_phys, sdma->regs + SDMA_H_C0PTR);
+	writel_relaxed(ccb_phys, sdma->regs + SDMA_H_C0PTR);
 
 	/* Set bits of CONFIG register with given context switching mode */
-	__raw_writel(SDMA_H_CONFIG_CSM, sdma->regs + SDMA_H_CONFIG);
+	writel_relaxed(SDMA_H_CONFIG_CSM, sdma->regs + SDMA_H_CONFIG);
 
 	/* Initializes channel's priorities */
 	sdma_set_channel_priority(&sdma->channel[0], 7);
diff --git a/drivers/dma/pxp/pxp_dma.c b/drivers/dma/pxp/pxp_dma.c
index eb5b4fb60306..ddd1c0cd192d 100644
--- a/drivers/dma/pxp/pxp_dma.c
+++ b/drivers/dma/pxp/pxp_dma.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010-2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2010-2012 Freescale Semiconductor, Inc.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -180,6 +180,7 @@ static bool is_yuv(u32 pix_fmt)
 	    (pix_fmt == PXP_PIX_FMT_YUV444) |
 	    (pix_fmt == PXP_PIX_FMT_NV12) |
 	    (pix_fmt == PXP_PIX_FMT_GREY) |
+	    (pix_fmt == PXP_PIX_FMT_GY04) |
 	    (pix_fmt == PXP_PIX_FMT_YVU410P) |
 	    (pix_fmt == PXP_PIX_FMT_YUV410P) |
 	    (pix_fmt == PXP_PIX_FMT_YVU420P) |
@@ -215,6 +216,9 @@ static void pxp_set_ctrl(struct pxps *pxp)
 	case PXP_PIX_FMT_GREY:
 		fmt_ctrl = BV_PXP_CTRL_S0_FORMAT__YUV420;
 		break;
+	case PXP_PIX_FMT_GY04:
+		fmt_ctrl = BV_PXP_CTRL_S0_FORMAT__YUV420;
+		break;
 	case PXP_PIX_FMT_YUV422P:
 		fmt_ctrl = BV_PXP_CTRL_S0_FORMAT__YUV422;
 		break;
@@ -243,6 +247,9 @@ static void pxp_set_ctrl(struct pxps *pxp)
 	case PXP_PIX_FMT_GREY:
 		fmt_ctrl = BV_PXP_CTRL_OUTBUF_FORMAT__MONOC8;
 		break;
+	case PXP_PIX_FMT_GY04:
+		fmt_ctrl = BV_PXP_CTRL_OUTBUF_FORMAT__MONOC4;
+		break;
 	default:
 		fmt_ctrl = 0;
 	}
@@ -620,6 +627,14 @@ static void pxp_set_s0buf(struct pxps *pxp)
 		__raw_writel(U, pxp->base + HW_PXP_S0UBUF);
 		__raw_writel(V, pxp->base + HW_PXP_S0VBUF);
 	}
+
+	/* TODO: only support RGB565, Y8 , Y4 */
+	if (s0_params->pixel_fmt == PXP_PIX_FMT_GREY)
+		__raw_writel(s0_params->width, pxp->base + HW_PXP_PS_PITCH);
+	else if (s0_params->pixel_fmt == PXP_PIX_FMT_GY04)
+		 __raw_writel(s0_params->width >> 1, pxp->base + HW_PXP_PS_PITCH);
+	else
+		__raw_writel(s0_params->width * 2, pxp->base + HW_PXP_PS_PITCH);
 }
 
 /**
diff --git a/drivers/dma/pxp/pxp_dma_v2.c b/drivers/dma/pxp/pxp_dma_v2.c
index 3fadd9a5f762..ceb72edbd39e 100644
--- a/drivers/dma/pxp/pxp_dma_v2.c
+++ b/drivers/dma/pxp/pxp_dma_v2.c
@@ -220,6 +220,7 @@ static bool is_yuv(u32 pix_fmt)
 	    (pix_fmt == PXP_PIX_FMT_YUV444) |
 	    (pix_fmt == PXP_PIX_FMT_NV12) |
 	    (pix_fmt == PXP_PIX_FMT_GREY) |
+	    (pix_fmt == PXP_PIX_FMT_GY04) |
 	    (pix_fmt == PXP_PIX_FMT_YVU410P) |
 	    (pix_fmt == PXP_PIX_FMT_YUV410P) |
 	    (pix_fmt == PXP_PIX_FMT_YVU420P) |
@@ -257,6 +258,9 @@ static void pxp_set_ctrl(struct pxps *pxp)
 	case PXP_PIX_FMT_GREY:
 		fmt_ctrl = BV_PXP_PS_CTRL_FORMAT__Y8;
 		break;
+	case PXP_PIX_FMT_GY04:
+		fmt_ctrl = BV_PXP_PS_CTRL_FORMAT__Y4;
+		break;
 	case PXP_PIX_FMT_YUV422P:
 		fmt_ctrl = BV_PXP_PS_CTRL_FORMAT__YUV422;
 		break;
@@ -287,6 +291,9 @@ static void pxp_set_ctrl(struct pxps *pxp)
 	case PXP_PIX_FMT_GREY:
 		fmt_ctrl = BV_PXP_OUT_CTRL_FORMAT__Y8;
 		break;
+	case PXP_PIX_FMT_GY04:
+		fmt_ctrl = BV_PXP_OUT_CTRL_FORMAT__Y4;
+		break;
 	default:
 		fmt_ctrl = 0;
 	}
@@ -697,9 +704,11 @@ static void pxp_set_s0buf(struct pxps *pxp)
 		__raw_writel(V, pxp->base + HW_PXP_PS_VBUF);
 	}
 
-	/* TODO: only support RGB565, Y8 */
+	/* TODO: only support RGB565, Y8, Y4 */
 	if (s0_params->pixel_fmt == PXP_PIX_FMT_GREY)
 		__raw_writel(s0_params->width, pxp->base + HW_PXP_PS_PITCH);
+	else if (s0_params->pixel_fmt == PXP_PIX_FMT_GY04)
+		__raw_writel(s0_params->width >> 1, pxp->base + HW_PXP_PS_PITCH);
 	else
 		__raw_writel(s0_params->width * 2, pxp->base + HW_PXP_PS_PITCH);
 }
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index bf8d1797589c..bf002d471b1f 100755
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -187,6 +187,18 @@ config TOUCHSCREEN_EGALAX
 	  To compile this driver as a module, choose M here: the
 	  module will be called egalax_ts.
 
+config TOUCHSCREEN_ELAN
+	tristate "ELAN touchscreen input driver"
+	depends on I2C
+	help
+	  Say Y here if you have an I2C ELAN touchscreen
+	  attached.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called elan-touch.
+
 config TOUCHSCREEN_FUJITSU
 	tristate "Fujitsu serial touchscreen"
 	select SERIO
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index f61dfe88bc62..28ac2d2c81bd 100755
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -66,3 +66,4 @@ obj-$(CONFIG_TOUCHSCREEN_P1003)		+= p1003_ts.o
 obj-$(CONFIG_TOUCHSCREEN_TPS6507X)	+= tps6507x-ts.o
 obj-$(CONFIG_TOUCHSCREEN_MAX11801)	+= max11801_ts.o
 obj-$(CONFIG_TOUCHSCREEN_EGALAX)	+= egalax_ts.o
+obj-$(CONFIG_TOUCHSCREEN_ELAN)		+= elan_ts.o
diff --git a/drivers/input/touchscreen/elan_ts.c b/drivers/input/touchscreen/elan_ts.c
new file mode 100644
index 000000000000..7edd87d6cd1b
--- /dev/null
+++ b/drivers/input/touchscreen/elan_ts.c
@@ -0,0 +1,387 @@
+/*
+ * Copyright (C) 2007-2008 HTC Corporation.
+ *
+ * This driver is adapted from elan8232_i2c.c written by Shan-Fu Chiou
+ * <sfchiou@gmail.com> and Jay Tu <jay_tu@htc.com>.
+ * This driver is also adapted from the ELAN Touch Screen driver
+ * written by Stanley Zeng <stanley.zeng@emc.com.tw>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/input.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/hrtimer.h>
+#include <linux/gpio.h>
+
+static const char ELAN_TS_NAME[] = "elan-touch";
+
+#define ELAN_TS_X_MAX 		1088
+#define ELAN_TS_Y_MAX 		768
+#define ELAN_USER_X_MAX		800
+#define ELAN_USER_Y_MAX		600
+#define IDX_PACKET_SIZE		8
+
+enum {
+	hello_packet = 0x55,
+	idx_coordinate_packet = 0x5a,
+};
+
+enum {
+	idx_finger_state = 7,
+};
+
+static struct workqueue_struct *elan_wq;
+
+static struct elan_data {
+	int intr_gpio;
+	int use_irq;
+	struct hrtimer timer;
+	struct work_struct work;
+	struct i2c_client *client;
+	struct input_dev *input;
+	wait_queue_head_t wait;
+} elan_touch_data;
+
+/*--------------------------------------------------------------*/
+static int elan_touch_detect_int_level(void)
+{
+	unsigned v;
+	v = gpio_get_value(elan_touch_data.intr_gpio);
+
+	return v;
+}
+
+static int __elan_touch_poll(struct i2c_client *client)
+{
+	int status = 0, retry = 20;
+
+	do {
+		status = elan_touch_detect_int_level();
+		retry--;
+		mdelay(20);
+	} while (status == 1 && retry > 0);
+
+	return (status == 0 ? 0 : -ETIMEDOUT);
+}
+
+static int elan_touch_poll(struct i2c_client *client)
+{
+	return __elan_touch_poll(client);
+}
+
+static int __hello_packet_handler(struct i2c_client *client)
+{
+	int rc;
+	uint8_t buf_recv[4] = { 0 };
+
+	rc = elan_touch_poll(client);
+
+	if (rc < 0) {
+		return -EINVAL;
+	}
+
+	rc = i2c_master_recv(client, buf_recv, 4);
+
+	if (rc != 4) {
+		return rc;
+	} else {
+		int i;
+		printk("hello packet: [0x%02x 0x%02x 0x%02x 0x%02x]\n",
+		       buf_recv[0], buf_recv[1], buf_recv[2], buf_recv[3]);
+
+		for (i = 0; i < 4; i++)
+			if (buf_recv[i] != hello_packet)
+				return -EINVAL;
+	}
+
+	return 0;
+}
+
+static inline int elan_touch_parse_xy(uint8_t *data, uint16_t *x,
+				      uint16_t *y)
+{
+	*x = (data[0] & 0xf0);
+	*x <<= 4;
+	*x |= data[1];
+	if (*x >= ELAN_TS_X_MAX)
+		*x = ELAN_TS_X_MAX;
+	*x = ((((ELAN_TS_X_MAX -
+		 *x) * 1000) / ELAN_TS_X_MAX) * ELAN_USER_X_MAX) / 1000;
+
+	*y = (data[0] & 0x0f);
+	*y <<= 8;
+	*y |= data[2];
+	if (*y >= ELAN_TS_Y_MAX)
+		*y = ELAN_TS_Y_MAX;
+	*y = ((((ELAN_TS_Y_MAX -
+		 *y) * 1000) / ELAN_TS_Y_MAX) * ELAN_USER_Y_MAX) / 1000;
+
+	return 0;
+}
+
+/*	__elan_touch_init -- hand shaking with touch panel
+ *
+ *	1.recv hello packet
+ */
+static int __elan_touch_init(struct i2c_client *client)
+{
+	int rc;
+	rc = __hello_packet_handler(client);
+	if (rc < 0)
+		goto hand_shake_failed;
+
+hand_shake_failed:
+	return rc;
+}
+
+static int elan_touch_recv_data(struct i2c_client *client, uint8_t * buf)
+{
+	int rc, bytes_to_recv = IDX_PACKET_SIZE;
+
+	if (buf == NULL)
+		return -EINVAL;
+
+	memset(buf, 0, bytes_to_recv);
+	rc = i2c_master_recv(client, buf, bytes_to_recv);
+	if (rc != bytes_to_recv) {
+		return -EINVAL;
+	}
+
+	return rc;
+}
+
+static void elan_touch_report_data(struct i2c_client *client, uint8_t * buf)
+{
+	switch (buf[0]) {
+	case idx_coordinate_packet:
+	{
+		uint16_t x1, x2, y1, y2;
+		uint8_t finger_stat;
+
+		finger_stat = (buf[idx_finger_state] & 0x06) >> 1;
+
+		if (finger_stat == 0) {
+			input_report_key(elan_touch_data.input, BTN_TOUCH, 0);
+			input_report_key(elan_touch_data.input, BTN_2, 0);
+		} else if (finger_stat == 1) {
+			elan_touch_parse_xy(&buf[1], &x1, &y1);
+			input_report_abs(elan_touch_data.input, ABS_X, x1);
+			input_report_abs(elan_touch_data.input, ABS_Y, y1);
+			input_report_key(elan_touch_data.input, BTN_TOUCH, 1);
+			input_report_key(elan_touch_data.input, BTN_2, 0);
+		} else if (finger_stat == 2) {
+			elan_touch_parse_xy(&buf[1], &x1, &y1);
+			input_report_abs(elan_touch_data.input, ABS_X, x1);
+			input_report_abs(elan_touch_data.input, ABS_Y, y1);
+			input_report_key(elan_touch_data.input, BTN_TOUCH, 1);
+			elan_touch_parse_xy(&buf[4], &x2, &y2);
+			input_report_abs(elan_touch_data.input, ABS_HAT0X, x2);
+			input_report_abs(elan_touch_data.input, ABS_HAT0Y, y2);
+			input_report_key(elan_touch_data.input, BTN_2, 1);
+		}
+		input_sync(elan_touch_data.input);
+		break;
+	}
+
+	default:
+		break;
+	}
+}
+
+static void elan_touch_work_func(struct work_struct *work)
+{
+	int rc;
+	uint8_t buf[IDX_PACKET_SIZE] = { 0 };
+	struct i2c_client *client = elan_touch_data.client;
+
+	if (elan_touch_detect_int_level())
+		return;
+
+	rc = elan_touch_recv_data(client, buf);
+	if (rc < 0)
+		return;
+
+	elan_touch_report_data(client, buf);
+}
+
+static irqreturn_t elan_touch_ts_interrupt(int irq, void *dev_id)
+{
+	queue_work(elan_wq, &elan_touch_data.work);
+
+	return IRQ_HANDLED;
+}
+
+static enum hrtimer_restart elan_touch_timer_func(struct hrtimer *timer)
+{
+	queue_work(elan_wq, &elan_touch_data.work);
+	hrtimer_start(&elan_touch_data.timer, ktime_set(0, 12500000),
+		      HRTIMER_MODE_REL);
+
+	return HRTIMER_NORESTART;
+}
+
+static int elan_touch_register_interrupt(struct i2c_client *client)
+{
+	int err = 0;
+
+	if (client->irq) {
+		elan_touch_data.use_irq = 1;
+		err =
+		    request_irq(client->irq, elan_touch_ts_interrupt,
+				IRQF_TRIGGER_FALLING, ELAN_TS_NAME,
+				&elan_touch_data);
+
+		if (err < 0) {
+			printk("%s(%s): Can't allocate irq %d\n", __FILE__,
+			       __func__, client->irq);
+			elan_touch_data.use_irq = 0;
+		}
+	}
+
+	if (!elan_touch_data.use_irq) {
+		hrtimer_init(&elan_touch_data.timer, CLOCK_MONOTONIC,
+			     HRTIMER_MODE_REL);
+		elan_touch_data.timer.function = elan_touch_timer_func;
+		hrtimer_start(&elan_touch_data.timer, ktime_set(1, 0),
+			      HRTIMER_MODE_REL);
+	}
+
+	printk("elan ts starts in %s mode.\n",
+	       elan_touch_data.use_irq == 1 ? "interrupt" : "polling");
+
+	return 0;
+}
+
+static int elan_touch_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	int err = 0;
+
+	elan_wq = create_singlethread_workqueue("elan_wq");
+	if (!elan_wq) {
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	elan_touch_data.client = client;
+	strlcpy(client->name, ELAN_TS_NAME, I2C_NAME_SIZE);
+
+	INIT_WORK(&elan_touch_data.work, elan_touch_work_func);
+
+	elan_touch_data.intr_gpio = irq_to_gpio(client->irq);
+	pr_debug("irq_to_gpio irq %d, gpio %d\n", client->irq,
+		 elan_touch_data.intr_gpio);
+
+	elan_touch_data.input = input_allocate_device();
+	if (elan_touch_data.input == NULL) {
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	err = __elan_touch_init(client);
+	if (err < 0) {
+		printk("elan - Read Hello Packet Failed\n");
+		goto fail;
+	}
+
+	elan_touch_data.input->name = ELAN_TS_NAME;
+	elan_touch_data.input->id.bustype = BUS_I2C;
+
+	set_bit(EV_SYN, elan_touch_data.input->evbit);
+
+	set_bit(EV_KEY, elan_touch_data.input->evbit);
+	set_bit(BTN_TOUCH, elan_touch_data.input->keybit);
+	set_bit(BTN_2, elan_touch_data.input->keybit);
+
+	set_bit(EV_ABS, elan_touch_data.input->evbit);
+	set_bit(ABS_X, elan_touch_data.input->absbit);
+	set_bit(ABS_Y, elan_touch_data.input->absbit);
+	set_bit(ABS_HAT0X, elan_touch_data.input->absbit);
+	set_bit(ABS_HAT0Y, elan_touch_data.input->absbit);
+
+	input_set_abs_params(elan_touch_data.input, ABS_X, 0, ELAN_USER_X_MAX,
+			     0, 0);
+	input_set_abs_params(elan_touch_data.input, ABS_Y, 0, ELAN_USER_Y_MAX,
+			     0, 0);
+	input_set_abs_params(elan_touch_data.input, ABS_HAT0X, 0,
+			     ELAN_USER_X_MAX, 0, 0);
+	input_set_abs_params(elan_touch_data.input, ABS_HAT0Y, 0,
+			     ELAN_USER_Y_MAX, 0, 0);
+
+	err = input_register_device(elan_touch_data.input);
+	if (err < 0) {
+		goto fail;
+	}
+
+	elan_touch_register_interrupt(elan_touch_data.client);
+
+	return 0;
+
+fail:
+	input_free_device(elan_touch_data.input);
+	if (elan_wq)
+		destroy_workqueue(elan_wq);
+	return err;
+}
+
+static int elan_touch_remove(struct i2c_client *client)
+{
+	if (elan_wq)
+		destroy_workqueue(elan_wq);
+
+	input_unregister_device(elan_touch_data.input);
+
+	if (elan_touch_data.use_irq)
+		free_irq(client->irq, client);
+	else
+		hrtimer_cancel(&elan_touch_data.timer);
+	return 0;
+}
+
+/* -------------------------------------------------------------------- */
+static const struct i2c_device_id elan_touch_id[] = {
+	{"elan-touch", 0},
+	{}
+};
+
+static struct i2c_driver elan_touch_driver = {
+	.probe = elan_touch_probe,
+	.remove = elan_touch_remove,
+	.id_table = elan_touch_id,
+	.driver = {
+		   .name = "elan-touch",
+		   .owner = THIS_MODULE,
+		   },
+};
+
+static int __init elan_touch_init(void)
+{
+	return i2c_add_driver(&elan_touch_driver);
+}
+
+static void __exit elan_touch_exit(void)
+{
+	i2c_del_driver(&elan_touch_driver);
+}
+
+module_init(elan_touch_init);
+module_exit(elan_touch_exit);
+
+MODULE_AUTHOR("Stanley Zeng <stanley.zeng@emc.com.tw>");
+MODULE_DESCRIPTION("ELAN Touch Screen driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/max11801_ts.c b/drivers/input/touchscreen/max11801_ts.c
index ca88d6823a98..48ee3600a03b 100755
--- a/drivers/input/touchscreen/max11801_ts.c
+++ b/drivers/input/touchscreen/max11801_ts.c
@@ -2,7 +2,7 @@
  * Driver for MAXI MAX11801 - A Resistive touch screen controller with
  * i2c interface
  *
- * Copyright (C) 2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2011-2012 Freescale Semiconductor, Inc.
  * Author: Zhang Jiejing <jiejing.zhang@freescale.com>
  *
  * Based on mcs5000_ts.c
@@ -38,6 +38,8 @@
 #include <linux/input.h>
 #include <linux/slab.h>
 #include <linux/bitops.h>
+#include <linux/delay.h>
+
 
 /* Register Address define */
 #define GENERNAL_STATUS_REG		0x00
@@ -53,13 +55,32 @@
 #define AUX_MESURE_CONF_REG		0x0a
 #define OP_MODE_CONF_REG		0x0b
 
+#define Panel_Setup_X	(0x69 << 1)
+#define Panel_Setup_Y	(0x6b << 1)
+
+#define XY_combined_measurement	(0x70 << 1)
+#define X_measurement	(0x78 << 1)
+#define Y_measurement	(0x7a << 1)
+#define AUX_measurement		(0x76 << 1)
+
 /* FIFO is found only in max11800 and max11801 */
 #define FIFO_RD_CMD			(0x50 << 1)
 #define MAX11801_FIFO_INT		(1 << 2)
 #define MAX11801_FIFO_OVERFLOW		(1 << 3)
+#define MAX11801_EDGE_INT      (1 << 1)
+
+#define FIFO_RD_X_MSB			(0x52 << 1)
+#define FIFO_RD_X_LSB			(0x53 << 1)
+#define FIFO_RD_Y_MSB			(0x54 << 1)
+#define FIFO_RD_Y_LSB			(0x55 << 1)
+#define FIFO_RD_AUX_MSB			(0x5a << 1)
+#define FIFO_RD_AUX_LSB			(0x5b << 1)
 
 #define XY_BUFSIZE			4
 #define XY_BUF_OFFSET			4
+#define X_BUFSIZE			2
+#define Y_BUFSIZE			2
+#define AUX_BUFSIZE			2
 
 #define MAX11801_MAX_X			0xfff
 #define MAX11801_MAX_Y			0xfff
@@ -83,6 +104,54 @@ struct max11801_data {
 	struct i2c_client		*client;
 	struct input_dev		*input_dev;
 };
+struct i2c_client *max11801_client;
+unsigned int max11801_workmode;
+
+static int max11801_dcm_write_command(struct i2c_client *client, int command)
+{
+	return i2c_smbus_write_byte(client, command);
+}
+
+static u32 max11801_dcm_sample_aux(struct i2c_client *client)
+{
+	u8 temp_buf;
+	u8 aux_buf[AUX_BUFSIZE];
+	int ret;
+	int aux = 0;
+	u32 sample_data = 0;
+	/* AUX_measurement*/
+	max11801_dcm_write_command(client, AUX_measurement);
+	mdelay(5);
+	ret = i2c_smbus_read_i2c_block_data(client, FIFO_RD_AUX_MSB,
+						1, &temp_buf);
+	aux_buf[0] = temp_buf;
+	if (ret < 1)
+		printk(KERN_DEBUG "FIFO_RD_AUX_MSB read fails\n");
+	mdelay(5);
+	ret = i2c_smbus_read_i2c_block_data(client, FIFO_RD_AUX_LSB,
+						1, &temp_buf);
+	aux_buf[1] = temp_buf;
+	if (ret < 1)
+		printk(KERN_DEBUG "FIFO_RD_AUX_LSB read fails\n");
+	aux = (aux_buf[0] << 4) +
+					(aux_buf[1] >> 4);
+	/*
+	voltage = (9170*aux)/7371;
+	voltage is (26.2*3150*aux)/(16.2*0xFFF)
+	V(aux)=3150*sample/0xFFF,V(battery)=212*V(aux)/81
+	sample_data = (14840*aux)/7371-1541;
+	*/
+	sample_data = (14840*aux)/7371;
+	return sample_data;
+}
+
+u32 max11801_read_adc(void)
+{
+	u32 adc_data;
+	adc_data = max11801_dcm_sample_aux(max11801_client);
+	return adc_data;
+}
+EXPORT_SYMBOL_GPL(max11801_read_adc);
 
 static u8 read_register(struct i2c_client *client, int addr)
 {
@@ -109,76 +178,166 @@ static irqreturn_t max11801_ts_interrupt(int irq, void *dev_id)
 	struct i2c_client *client = data->client;
 	int status, i, ret;
 	u8 buf[XY_BUFSIZE];
+	u8 x_buf[X_BUFSIZE];
+	u8 y_buf[Y_BUFSIZE];
+	u8 temp_buf[1];
 	int x = -1;
 	int y = -1;
 
 	status = read_register(data->client, GENERNAL_STATUS_REG);
-
-	if (status & (MAX11801_FIFO_INT | MAX11801_FIFO_OVERFLOW)) {
-		status = read_register(data->client, GENERNAL_STATUS_REG);
-
-		ret = i2c_smbus_read_i2c_block_data(client, FIFO_RD_CMD,
-						    XY_BUFSIZE, buf);
-
-		/*
-		 * We should get 4 bytes buffer that contains X,Y
-		 * and event tag
-		 */
-		if (ret < XY_BUFSIZE)
-			goto out;
-
-		for (i = 0; i < XY_BUFSIZE; i += XY_BUFSIZE / 2) {
-			if ((buf[i + 1] & MEASURE_TAG_MASK) == MEASURE_X_TAG)
-				x = (buf[i] << XY_BUF_OFFSET) +
-				    (buf[i + 1] >> XY_BUF_OFFSET);
-			else if ((buf[i + 1] & MEASURE_TAG_MASK) == MEASURE_Y_TAG)
-				y = (buf[i] << XY_BUF_OFFSET) +
-				    (buf[i + 1] >> XY_BUF_OFFSET);
-		}
-
-		if ((buf[1] & EVENT_TAG_MASK) != (buf[3] & EVENT_TAG_MASK))
-			goto out;
-
-		switch (buf[1] & EVENT_TAG_MASK) {
-		case EVENT_INIT:
-			/* fall through */
-		case EVENT_MIDDLE:
-			calibration_pointer(&x, &y);
-			input_report_abs(data->input_dev, ABS_X, x);
-			input_report_abs(data->input_dev, ABS_Y, y);
-			input_event(data->input_dev, EV_KEY, BTN_TOUCH, 1);
-			input_sync(data->input_dev);
-			break;
-
-		case EVENT_RELEASE:
-			input_event(data->input_dev, EV_KEY, BTN_TOUCH, 0);
-			input_sync(data->input_dev);
-			break;
-
-		case EVENT_FIFO_END:
-			break;
-		}
-	}
+	if (max11801_workmode == 0) {
+		if (status & (MAX11801_FIFO_INT | MAX11801_FIFO_OVERFLOW)) {
+				status = read_register(data->client, GENERNAL_STATUS_REG);
+
+				ret = i2c_smbus_read_i2c_block_data(client, FIFO_RD_CMD,
+								    XY_BUFSIZE, buf);
+
+				/*
+				 * We should get 4 bytes buffer that contains X,Y
+				 * and event tag
+				 */
+				if (ret < XY_BUFSIZE)
+					goto out;
+
+				for (i = 0; i < XY_BUFSIZE; i += XY_BUFSIZE / 2) {
+					if ((buf[i + 1] & MEASURE_TAG_MASK) == MEASURE_X_TAG)
+						x = (buf[i] << XY_BUF_OFFSET) +
+						    (buf[i + 1] >> XY_BUF_OFFSET);
+					else if ((buf[i + 1] & MEASURE_TAG_MASK) == MEASURE_Y_TAG)
+						y = (buf[i] << XY_BUF_OFFSET) +
+						    (buf[i + 1] >> XY_BUF_OFFSET);
+				}
+
+				if ((buf[1] & EVENT_TAG_MASK) != (buf[3] & EVENT_TAG_MASK))
+					goto out;
+
+				switch (buf[1] & EVENT_TAG_MASK) {
+				case EVENT_INIT:
+					/* fall through */
+				case EVENT_MIDDLE:
+					calibration_pointer(&x, &y);
+					input_report_abs(data->input_dev, ABS_X, x);
+					input_report_abs(data->input_dev, ABS_Y, y);
+					input_event(data->input_dev, EV_KEY, BTN_TOUCH, 1);
+					input_sync(data->input_dev);
+					break;
+
+				case EVENT_RELEASE:
+					input_event(data->input_dev, EV_KEY, BTN_TOUCH, 0);
+					input_sync(data->input_dev);
+					break;
+
+				case EVENT_FIFO_END:
+					break;
+				}
+			}
 out:
-	return IRQ_HANDLED;
+return IRQ_HANDLED;
+		}
+	else if (max11801_workmode == 1) {
+		if (status & (MAX11801_EDGE_INT)) {
+				status = read_register(data->client, GENERNAL_STATUS_REG);
+
+				/* X = panel setup*/
+				max11801_dcm_write_command(client, Panel_Setup_X);
+				/* X_measurement*/
+				max11801_dcm_write_command(client, X_measurement);
+				ret = i2c_smbus_read_i2c_block_data(client, FIFO_RD_X_MSB,
+									1, temp_buf);
+				x_buf[0] = temp_buf[0];
+				if (ret < 1)
+					goto out2;
+				ret = i2c_smbus_read_i2c_block_data(client, FIFO_RD_X_LSB,
+									1, temp_buf);
+				x_buf[1] = temp_buf[0];
+				if (ret < 1)
+					goto out2;
+				/* Y = panel setup*/
+				max11801_dcm_write_command(client, Panel_Setup_Y);
+				/* Y_measurement*/
+				max11801_dcm_write_command(client, Y_measurement);
+				ret = i2c_smbus_read_i2c_block_data(client, FIFO_RD_Y_MSB,
+									1, temp_buf);
+				y_buf[0] = temp_buf[0];
+				if (ret < 1)
+					goto out2;
+				ret = i2c_smbus_read_i2c_block_data(client, FIFO_RD_Y_LSB,
+									1, temp_buf);
+				y_buf[1] = temp_buf[0];
+				if (ret < 1)
+					goto out2;
+
+				if ((x_buf[1] & MEASURE_TAG_MASK) == MEASURE_X_TAG)
+					x = (x_buf[0] << XY_BUF_OFFSET) +
+						(x_buf[1] >> XY_BUF_OFFSET);
+				if ((y_buf[1] & MEASURE_TAG_MASK) == MEASURE_Y_TAG)
+					y = (y_buf[0] << XY_BUF_OFFSET) +
+						(y_buf[1] >> XY_BUF_OFFSET);
+
+				if ((x_buf[1] & EVENT_TAG_MASK) != (y_buf[1] & EVENT_TAG_MASK))
+					goto out2;
+
+				switch (x_buf[1] & EVENT_TAG_MASK) {
+				case EVENT_INIT:
+					/* fall through */
+				case EVENT_MIDDLE:
+					calibration_pointer(&x, &y);
+					input_report_abs(data->input_dev, ABS_X, x);
+					input_report_abs(data->input_dev, ABS_Y, y);
+					input_event(data->input_dev, EV_KEY, BTN_TOUCH, 1);
+					input_sync(data->input_dev);
+					break;
+
+				case EVENT_RELEASE:
+					input_event(data->input_dev, EV_KEY, BTN_TOUCH, 0);
+					input_sync(data->input_dev);
+					break;
+
+				case EVENT_FIFO_END:
+					break;
+				}
+			  }
+			}
+out2:
+return IRQ_HANDLED;
 }
 
 static void __devinit max11801_ts_phy_init(struct max11801_data *data)
 {
 	struct i2c_client *client = data->client;
-
-	/* Average X,Y, take 16 samples, average eight media sample */
-	max11801_write_reg(client, MESURE_AVER_CONF_REG, 0xff);
-	/* X,Y panel setup time set to 20us */
-	max11801_write_reg(client, PANEL_SETUPTIME_CONF_REG, 0x11);
-	/* Rough pullup time (2uS), Fine pullup time (10us)  */
-	max11801_write_reg(client, TOUCH_DETECT_PULLUP_CONF_REG, 0x10);
-	/* Auto mode init period = 5ms , scan period = 5ms*/
-	max11801_write_reg(client, AUTO_MODE_TIME_CONF_REG, 0xaa);
-	/* Aperture X,Y set to +- 4LSB */
-	max11801_write_reg(client, APERTURE_CONF_REG, 0x33);
-	/* Enable Power, enable Automode, enable Aperture, enable Average X,Y */
-	max11801_write_reg(client, OP_MODE_CONF_REG, 0x36);
+	max11801_client = client;
+	if (max11801_workmode == 0) {
+		/* Average X,Y, take 16 samples, average eight media sample */
+		max11801_write_reg(client, MESURE_AVER_CONF_REG, 0xff);
+		/* X,Y panel setup time set to 20us */
+		max11801_write_reg(client, PANEL_SETUPTIME_CONF_REG, 0x11);
+		/* Rough pullup time (2uS), Fine pullup time (10us)  */
+		max11801_write_reg(client, TOUCH_DETECT_PULLUP_CONF_REG, 0x10);
+		/* Auto mode init period = 5ms , scan period = 5ms*/
+		max11801_write_reg(client, AUTO_MODE_TIME_CONF_REG, 0xaa);
+		/* Aperture X,Y set to +- 4LSB */
+		max11801_write_reg(client, APERTURE_CONF_REG, 0x33);
+		/* Enable Power, enable Automode, enable Aperture, enable Average X,Y */
+		max11801_write_reg(client, OP_MODE_CONF_REG, 0x36);
+	}
+	if (max11801_workmode == 1) {
+		/* Average X,Y, take 16 samples, average eight media sample */
+		max11801_write_reg(client, MESURE_AVER_CONF_REG, 0xff);
+		/* X,Y panel setup time set to 20us */
+		max11801_write_reg(client, PANEL_SETUPTIME_CONF_REG, 0x11);
+		/* Rough pullup time (2uS), Fine pullup time (10us)  */
+		max11801_write_reg(client, TOUCH_DETECT_PULLUP_CONF_REG, 0x10);
+		/* Auto mode init period = 5ms , scan period = 5ms*/
+		max11801_write_reg(client, AUTO_MODE_TIME_CONF_REG, 0xaa);
+		/* Aperture X,Y set to +- 4LSB */
+		max11801_write_reg(client, APERTURE_CONF_REG, 0x33);
+		/* Enable Power, enable Direct conversion mode , enable Aperture, enable Average X,Y */
+		max11801_write_reg(client, OP_MODE_CONF_REG, 0x16);
+		/* Delay initial=1ms, Sampling time 2us ,Averaging sample depth 2 samples, Resolution 12bit */
+		max11801_write_reg(client, AUX_MESURE_CONF_REG, 0x76);
+		/* Use edge interrupt with direct conversion mode  */
+		max11801_write_reg(client, GENERNAL_CONF_REG, 0xf3);
+	}
 }
 
 static int __devinit max11801_ts_probe(struct i2c_client *client,
@@ -195,7 +354,7 @@ static int __devinit max11801_ts_probe(struct i2c_client *client,
 		error = -ENOMEM;
 		goto err_free_mem;
 	}
-
+	max11801_workmode = *(int *)(client->dev).platform_data;
 	data->client = client;
 	data->input_dev = input_dev;
 
diff --git a/drivers/media/video/mxc/capture/ipu_prp_enc.c b/drivers/media/video/mxc/capture/ipu_prp_enc.c
index 6710cba5d817..02515b35ba03 100644
--- a/drivers/media/video/mxc/capture/ipu_prp_enc.c
+++ b/drivers/media/video/mxc/capture/ipu_prp_enc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -170,7 +170,8 @@ static int prp_enc_setup(cam_data *cam)
 		return err;
 	}
 
-	ipu_csi_enable_mclk_if(cam->ipu, CSI_MCLK_ENC, cam->csi, true, true);
+	ipu_csi_enable_mclk_if(cam->ipu, CSI_MCLK_ENC,
+				cam->mclk_source, true, true);
 
 	grotation = cam->rotation;
 	if (cam->rotation >= IPU_ROTATE_90_RIGHT) {
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
index 7713b3d80846..c2bda758a362 100644
--- a/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.c
@@ -32,19 +32,19 @@
 #include <linux/types.h>
 #include <linux/fb.h>
 #include <linux/dma-mapping.h>
+#include <linux/delay.h>
 #include <linux/mxcfb.h>
 #include <media/v4l2-chip-ident.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-int-device.h>
+#include <linux/fsl_devices.h>
 #include "mxc_v4l2_capture.h"
 #include "ipu_prp_sw.h"
 
 #define init_MUTEX(sem)         sema_init(sem, 1)
 #define MXC_SENSOR_NUM 2
-static int sensor_index;
 
-static int video_nr = -1, local_buf_num;
-static cam_data *g_cam;
+static int video_nr = -1;
 
 /*! This data is used for the output to the display. */
 #define MXC_V4L2_CAPTURE_NUM_OUTPUTS	3
@@ -404,7 +404,7 @@ static int mxc_streamon(cam_data *cam)
 	}
 
 	cam->ping_pong_csi = 0;
-	local_buf_num = 0;
+	cam->local_buf_num = 0;
 	if (cam->enc_update_eba) {
 		frame =
 		    list_entry(cam->ready_q.next, struct mxc_v4l_frame, queue);
@@ -1229,7 +1229,7 @@ static int mxc_v4l2_s_ctrl(cam_data *cam, struct v4l2_control *c)
 	case V4L2_CID_MXC_SWITCH_CAM:
 		if (cam->sensor != cam->all_sensors[c->value]) {
 			/* power down other cameraes before enable new one */
-			for (i = 0; i < sensor_index; i++) {
+			for (i = 0; i < cam->sensor_index; i++) {
 				if (i != c->value) {
 					vidioc_int_dev_exit(cam->all_sensors[i]);
 					vidioc_int_s_power(cam->all_sensors[i], 0);
@@ -1684,12 +1684,13 @@ static int mxc_v4l_open(struct file *file)
 					cam_fmt.fmt.pix.pixelformat,
 					csi_param);
 
-		ipu_csi_enable_mclk_if(cam->ipu, CSI_MCLK_I2C, cam->csi,
+		ipu_csi_enable_mclk_if(cam->ipu, CSI_MCLK_I2C, cam->mclk_source,
 				       true, true);
 		vidioc_int_s_power(cam->sensor, 1);
+		msleep(1);
 		vidioc_int_init(cam->sensor);
 		vidioc_int_dev_init(cam->sensor);
-}
+	}
 
 	file->private_data = dev;
 
@@ -2519,7 +2520,7 @@ static void camera_callback(u32 mask, void *dev)
 					struct mxc_v4l_frame,
 					queue);
 
-		if (done_frame->ipu_buf_num != local_buf_num)
+		if (done_frame->ipu_buf_num != cam->local_buf_num)
 			goto next;
 
 		/*
@@ -2556,7 +2557,7 @@ next:
 				list_del(cam->ready_q.next);
 				list_add_tail(&ready_frame->queue,
 					      &cam->working_q);
-				ready_frame->ipu_buf_num = local_buf_num;
+				ready_frame->ipu_buf_num = cam->local_buf_num;
 			}
 	} else {
 		if (cam->enc_update_eba)
@@ -2565,7 +2566,7 @@ next:
 				&cam->ping_pong_csi);
 	}
 
-	local_buf_num = (local_buf_num == 0) ? 1 : 0;
+	cam->local_buf_num = (cam->local_buf_num == 0) ? 1 : 0;
 
 	return;
 }
@@ -2579,11 +2580,13 @@ next:
  */
 static void init_camera_struct(cam_data *cam, struct platform_device *pdev)
 {
+	struct fsl_mxc_capture_platform_data *pdata = pdev->dev.platform_data;
+
 	pr_debug("In MVC: init_camera_struct\n");
 	/* Default everything to 0 */
 	memset(cam, 0, sizeof(cam_data));
 
-	cam->ipu = ipu_get_soc(0);
+	cam->ipu = ipu_get_soc(pdata->ipu);
 	if (cam->ipu == NULL)
 		pr_err("ERROR: v4l2 capture: failed to get ipu\n");
 	else if (cam->ipu == ERR_PTR(-ENODEV))
@@ -2640,8 +2643,8 @@ static void init_camera_struct(cam_data *cam, struct platform_device *pdev)
 	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->csi = pdata->csi;
+	cam->mclk_source = pdata->mclk_source;
 
 	cam->enc_callback = camera_callback;
 	init_waitqueue_head(&cam->power_queue);
@@ -2655,6 +2658,12 @@ static void init_camera_struct(cam_data *cam, struct platform_device *pdev)
 		pr_err("ERROR: v4l2 capture: Allocate dummy frame "
 		       "failed.\n");
 	cam->dummy_frame.buffer.length = SZ_8M;
+
+	cam->self = kmalloc(sizeof(struct v4l2_int_device), GFP_KERNEL);
+	cam->self->module = THIS_MODULE;
+	sprintf(cam->self->name, "mxc_v4l2_cap%d", cam->csi);
+	cam->self->type = v4l2_int_type_master;
+	cam->self->u.master = &mxc_v4l2_master;
 }
 
 static ssize_t show_streaming(struct device *dev,
@@ -2662,9 +2671,9 @@ static ssize_t show_streaming(struct device *dev,
 {
 	struct video_device *video_dev = container_of(dev,
 						struct video_device, dev);
-	cam_data *g_cam = video_get_drvdata(video_dev);
+	cam_data *cam = video_get_drvdata(video_dev);
 
-	if (g_cam->capture_on)
+	if (cam->capture_on)
 		return sprintf(buf, "stream on\n");
 	else
 		return sprintf(buf, "stream off\n");
@@ -2676,9 +2685,9 @@ static ssize_t show_overlay(struct device *dev,
 {
 	struct video_device *video_dev = container_of(dev,
 						struct video_device, dev);
-	cam_data *g_cam = video_get_drvdata(video_dev);
+	cam_data *cam = video_get_drvdata(video_dev);
 
-	if (g_cam->overlay_on)
+	if (cam->overlay_on)
 		return sprintf(buf, "overlay on\n");
 	else
 		return sprintf(buf, "overlay off\n");
@@ -2695,38 +2704,38 @@ static DEVICE_ATTR(fsl_v4l2_overlay_property, S_IRUGO, show_overlay, NULL);
  */
 static int mxc_v4l2_probe(struct platform_device *pdev)
 {
-	/* Create g_cam and initialize it. */
-	g_cam = kmalloc(sizeof(cam_data), GFP_KERNEL);
-	if (g_cam == NULL) {
+	/* Create cam and initialize it. */
+	cam_data *cam = kmalloc(sizeof(cam_data), GFP_KERNEL);
+	if (cam == NULL) {
 		pr_err("ERROR: v4l2 capture: failed to register camera\n");
 		return -1;
 	}
 
-	init_camera_struct(g_cam, pdev);
+	init_camera_struct(cam, pdev);
 	pdev->dev.release = camera_platform_release;
 
 	/* Set up the v4l2 device and register it*/
-	mxc_v4l2_int_device.priv = g_cam;
+	cam->self->priv = cam;
 	/* This function contains a bug that won't let this be rmmod'd. */
-	v4l2_int_device_register(&mxc_v4l2_int_device);
+	v4l2_int_device_register(cam->self);
 
 	/* register v4l video device */
-	if (video_register_device(g_cam->video_dev, VFL_TYPE_GRABBER, video_nr)
+	if (video_register_device(cam->video_dev, VFL_TYPE_GRABBER, video_nr)
 	    == -1) {
-		kfree(g_cam);
-		g_cam = NULL;
+		kfree(cam);
+		cam = NULL;
 		pr_err("ERROR: v4l2 capture: video_register_device failed\n");
 		return -1;
 	}
 	pr_debug("   Video device registered: %s #%d\n",
-		 g_cam->video_dev->name, g_cam->video_dev->minor);
+		 cam->video_dev->name, cam->video_dev->minor);
 
-	if (device_create_file(&g_cam->video_dev->dev,
+	if (device_create_file(&cam->video_dev->dev,
 			&dev_attr_fsl_v4l2_capture_property))
 		dev_err(&pdev->dev, "Error on creating sysfs file"
 			" for capture\n");
 
-	if (device_create_file(&g_cam->video_dev->dev,
+	if (device_create_file(&cam->video_dev->dev,
 			&dev_attr_fsl_v4l2_overlay_property))
 		dev_err(&pdev->dev, "Error on creating sysfs file"
 			" for overlay\n");
@@ -2744,31 +2753,31 @@ static int mxc_v4l2_probe(struct platform_device *pdev)
  */
 static int mxc_v4l2_remove(struct platform_device *pdev)
 {
+	cam_data *cam = (cam_data *)platform_get_drvdata(pdev);
 
-	if (g_cam->dummy_frame.vaddress != 0) {
-		dma_free_coherent(0, g_cam->dummy_frame.buffer.length,
-				  g_cam->dummy_frame.vaddress,
-				  g_cam->dummy_frame.paddress);
-		g_cam->dummy_frame.vaddress = 0;
+	if (cam->dummy_frame.vaddress != 0) {
+		dma_free_coherent(0, cam->dummy_frame.buffer.length,
+				  cam->dummy_frame.vaddress,
+				  cam->dummy_frame.paddress);
+		cam->dummy_frame.vaddress = 0;
 	}
 
-	if (g_cam->open_count) {
+	if (cam->open_count) {
 		pr_err("ERROR: v4l2 capture:camera open "
 			"-- setting ops to NULL\n");
 		return -EBUSY;
 	} else {
-		device_remove_file(&g_cam->video_dev->dev,
+		device_remove_file(&cam->video_dev->dev,
 			&dev_attr_fsl_v4l2_capture_property);
-		device_remove_file(&g_cam->video_dev->dev,
+		device_remove_file(&cam->video_dev->dev,
 			&dev_attr_fsl_v4l2_overlay_property);
 
 		pr_info("V4L2 freeing image input device\n");
 		v4l2_int_device_unregister(&mxc_v4l2_int_device);
-		video_unregister_device(g_cam->video_dev);
+		video_unregister_device(cam->video_dev);
 
-		mxc_free_frame_buf(g_cam);
-		kfree(g_cam);
-		g_cam = NULL;
+		mxc_free_frame_buf(cam);
+		kfree(cam);
 	}
 
 	pr_info("V4L2 unregistering video\n");
@@ -2865,6 +2874,7 @@ static int mxc_v4l2_master_attach(struct v4l2_int_device *slave)
 	cam_data *cam = slave->u.slave->master->priv;
 	struct v4l2_format cam_fmt;
 	int i;
+	struct sensor_data *sdata = slave->priv;
 
 	pr_debug("In MVC: mxc_v4l2_master_attach\n");
 	pr_debug("   slave.name = %s\n", slave->name);
@@ -2875,17 +2885,22 @@ static int mxc_v4l2_master_attach(struct v4l2_int_device *slave)
 		return -1;
 	}
 
+	if (sdata->csi != cam->csi) {
+		pr_debug("%s: csi doesn't match\n", __func__);
+		return -1;
+	}
+
 	cam->sensor = slave;
 
-	if (sensor_index < MXC_SENSOR_NUM) {
-		cam->all_sensors[sensor_index] = slave;
-		sensor_index++;
+	if (cam->sensor_index < MXC_SENSOR_NUM) {
+		cam->all_sensors[cam->sensor_index] = slave;
+		cam->sensor_index++;
 	} else {
 		pr_err("ERROR: v4l2 capture: slave number exceeds the maximum.\n");
 		return -1;
 	}
 
-	for (i = 0; i < sensor_index; i++) {
+	for (i = 0; i < cam->sensor_index; i++) {
 		vidioc_int_dev_exit(cam->all_sensors[i]);
 		vidioc_int_s_power(cam->all_sensors[i], 0);
 	}
@@ -2937,23 +2952,23 @@ static void mxc_v4l2_master_detach(struct v4l2_int_device *slave)
 
 	pr_debug("In MVC:mxc_v4l2_master_detach\n");
 
-	if (sensor_index > 1) {
-		for (i = 0; i < sensor_index; i++) {
+	if (cam->sensor_index > 1) {
+		for (i = 0; i < cam->sensor_index; i++) {
 			if (cam->all_sensors[i] != slave)
 				continue;
 			/* Move all the sensors behind this
 			 * sensor one step forward
 			 */
-			for (; i < sensor_index - 1; i++)
+			for (; i < cam->sensor_index - 1; i++)
 				cam->all_sensors[i] = cam->all_sensors[i+1];
 			break;
 		}
 		/* Point current sensor to the last one */
-		cam->sensor = cam->all_sensors[sensor_index - 2];
+		cam->sensor = cam->all_sensors[cam->sensor_index - 2];
 	} else
 		cam->sensor = NULL;
 
-	sensor_index--;
+	cam->sensor_index--;
 	vidioc_int_dev_exit(slave);
 }
 
diff --git a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
index 5b7b0bfe7706..dcb90a9f4ce5 100644
--- a/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
+++ b/drivers/media/video/mxc/capture/mxc_v4l2_capture.h
@@ -34,6 +34,8 @@
 #include <mach/ipu-v3.h>
 
 #include <media/v4l2-dev.h>
+#include <media/v4l2-int-device.h>
+
 
 #define FRAME_NUM 10
 
@@ -190,12 +192,17 @@ typedef struct _cam_data {
 	bool low_power;
 	wait_queue_head_t power_queue;
 	unsigned int csi;
+	u8 mclk_source;
 	int current_input;
 
+	int local_buf_num;
+
 	/* camera sensor interface */
 	struct camera_sensor *cam_sensor; 	/* old version */
 	struct v4l2_int_device *all_sensors[2];
 	struct v4l2_int_device *sensor;
+	struct v4l2_int_device *self;
+	int sensor_index;
 	void *ipu;
 } cam_data;
 
@@ -218,6 +225,7 @@ struct sensor_data {
 	int ae_mode;
 
 	u32 mclk;
+	u8 mclk_source;
 	int csi;
 
 	void (*io_init)(void);
diff --git a/drivers/media/video/mxc/capture/ov3640.c b/drivers/media/video/mxc/capture/ov3640.c
index b1d1abb884d9..69a3ad43b22c 100644
--- a/drivers/media/video/mxc/capture/ov3640.c
+++ b/drivers/media/video/mxc/capture/ov3640.c
@@ -1354,7 +1354,7 @@ static int ioctl_dev_init(struct v4l2_int_device *s)
 	ov3640_data.mclk = tgt_xclk;
 
 	pr_debug("   Setting mclk to %d MHz\n", tgt_xclk / 1000000);
-	set_mclk_rate(&ov3640_data.mclk, ov3640_data.csi);
+	set_mclk_rate(&ov3640_data.mclk, ov3640_data.mclk_source);
 
 	/* Default camera frame rate is set in probe */
 	tgt_fps = sensor->streamcap.timeperframe.denominator /
@@ -1442,6 +1442,7 @@ static int ov3640_probe(struct i2c_client *client,
 	memset(&ov3640_data, 0, sizeof(ov3640_data));
 	ov3640_data.mclk = 24000000; /* 6 - 54 MHz, typical 24MHz */
 	ov3640_data.mclk = plat_data->mclk;
+	ov3640_data.mclk_source = plat_data->mclk_source;
 	ov3640_data.csi = plat_data->csi;
 	ov3640_data.io_init = plat_data->io_init;
 
diff --git a/drivers/media/video/mxc/capture/ov5640.c b/drivers/media/video/mxc/capture/ov5640.c
index f9087499b5f1..c2dfe314c65d 100644
--- a/drivers/media/video/mxc/capture/ov5640.c
+++ b/drivers/media/video/mxc/capture/ov5640.c
@@ -1309,7 +1309,7 @@ static int ioctl_dev_init(struct v4l2_int_device *s)
 	ov5640_data.mclk = tgt_xclk;
 
 	pr_debug("   Setting mclk to %d MHz\n", tgt_xclk / 1000000);
-	set_mclk_rate(&ov5640_data.mclk, ov5640_data.csi);
+	set_mclk_rate(&ov5640_data.mclk, ov5640_data.mclk_source);
 
 	/* Default camera frame rate is set in probe */
 	tgt_fps = sensor->streamcap.timeperframe.denominator /
@@ -1397,6 +1397,7 @@ static int ov5640_probe(struct i2c_client *client,
 	memset(&ov5640_data, 0, sizeof(ov5640_data));
 	ov5640_data.mclk = 24000000; /* 6 - 54 MHz, typical 24MHz */
 	ov5640_data.mclk = plat_data->mclk;
+	ov5640_data.mclk_source = plat_data->mclk_source;
 	ov5640_data.csi = plat_data->csi;
 	ov5640_data.io_init = plat_data->io_init;
 
diff --git a/drivers/media/video/mxc/capture/ov5640_mipi.c b/drivers/media/video/mxc/capture/ov5640_mipi.c
index 2d9d83b7b139..9ffca952cc29 100644
--- a/drivers/media/video/mxc/capture/ov5640_mipi.c
+++ b/drivers/media/video/mxc/capture/ov5640_mipi.c
@@ -195,7 +195,7 @@ static struct reg_value ov5640_setting_30fps_VGA_640_480[] = {
 };
 
 static struct reg_value ov5640_setting_15fps_VGA_640_480[] = {
-	{0x3035, 0x24, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
 	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
@@ -237,7 +237,7 @@ static struct reg_value ov5640_setting_30fps_QVGA_320_240[] = {
 };
 
 static struct reg_value ov5640_setting_15fps_QVGA_320_240[] = {
-	{0x3035, 0x24, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
+	{0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 	{0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 	{0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
 	{0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
@@ -909,6 +909,15 @@ bool binning_on(void)
 		return false;
 }
 
+static void ov5640_set_virtual_channel(int channel)
+{
+	u8 channel_id;
+
+	ov5640_read_reg(0x4814, &channel_id);
+	channel_id &= ~(3 << 6);
+	ov5640_write_reg(0x4814, channel_id | (channel << 6));
+}
+
 static int ov5640_init_mode(enum ov5640_frame_rate frame_rate,
 			    enum ov5640_mode mode)
 {
@@ -1164,6 +1173,7 @@ static int ov5640_init_mode(enum ov5640_frame_rate frame_rate,
 	OV5640_set_AE_target(AE_Target);
 	OV5640_get_light_frequency();
 	OV5640_set_bandingfilter();
+	ov5640_set_virtual_channel(ov5640_data.csi);
 
 	if (mipi_csi2_info) {
 		unsigned int i;
@@ -1604,7 +1614,7 @@ static int ioctl_dev_init(struct v4l2_int_device *s)
 	ov5640_data.mclk = tgt_xclk;
 
 	pr_debug("   Setting mclk to %d MHz\n", tgt_xclk / 1000000);
-	set_mclk_rate(&ov5640_data.mclk, ov5640_data.csi);
+	set_mclk_rate(&ov5640_data.mclk, ov5640_data.mclk_source);
 
 	/* Default camera frame rate is set in probe */
 	tgt_fps = sensor->streamcap.timeperframe.denominator /
@@ -1712,6 +1722,7 @@ static int ov5640_probe(struct i2c_client *client,
 	memset(&ov5640_data, 0, sizeof(ov5640_data));
 	ov5640_data.mclk = 24000000; /* 6 - 54 MHz, typical 24MHz */
 	ov5640_data.mclk = plat_data->mclk;
+	ov5640_data.mclk_source = plat_data->mclk_source;
 	ov5640_data.csi = plat_data->csi;
 	ov5640_data.io_init = plat_data->io_init;
 
diff --git a/drivers/media/video/mxc/capture/ov5642.c b/drivers/media/video/mxc/capture/ov5642.c
index f8f8a6c12ab4..58ee92eef678 100644
--- a/drivers/media/video/mxc/capture/ov5642.c
+++ b/drivers/media/video/mxc/capture/ov5642.c
@@ -279,7 +279,7 @@ static struct reg_value ov5642_initial_setting[] = {
 	{0x5007, 0x00, 0, 0}, {0x5009, 0x00, 0, 0}, {0x5011, 0x00, 0, 0},
 	{0x5013, 0x00, 0, 0}, {0x519e, 0x00, 0, 0}, {0x5086, 0x00, 0, 0},
 	{0x5087, 0x00, 0, 0}, {0x5088, 0x00, 0, 0}, {0x5089, 0x00, 0, 0},
-	{0x302b, 0x00, 0, 0},
+	{0x302b, 0x00, 0, 300},
 };
 
 static struct reg_value ov5642_setting_15fps_QCIF_176_144[] = {
@@ -3255,7 +3255,7 @@ static int ioctl_dev_init(struct v4l2_int_device *s)
 	ov5642_data.mclk = tgt_xclk;
 
 	pr_debug("   Setting mclk to %d MHz\n", tgt_xclk / 1000000);
-	set_mclk_rate(&ov5642_data.mclk, ov5642_data.csi);
+	set_mclk_rate(&ov5642_data.mclk, ov5642_data.mclk_source);
 
 	/* Default camera frame rate is set in probe */
 	tgt_fps = sensor->streamcap.timeperframe.denominator /
@@ -3368,6 +3368,7 @@ static int ov5642_probe(struct i2c_client *client,
 	memset(&ov5642_data, 0, sizeof(ov5642_data));
 	ov5642_data.mclk = 24000000; /* 6 - 54 MHz, typical 24MHz */
 	ov5642_data.mclk = plat_data->mclk;
+	ov5642_data.mclk_source = plat_data->mclk_source;
 	ov5642_data.csi = plat_data->csi;
 	ov5642_data.io_init = plat_data->io_init;
 
diff --git a/drivers/media/video/mxc/capture/ov8820_mipi.c b/drivers/media/video/mxc/capture/ov8820_mipi.c
index 4c2a49d8bef5..6fddbb03fc0b 100644
--- a/drivers/media/video/mxc/capture/ov8820_mipi.c
+++ b/drivers/media/video/mxc/capture/ov8820_mipi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -92,6 +92,7 @@ struct sensor {
 	int ae_mode;
 
 	u32 mclk;
+	u8 mclk_source;
 	int csi;
 } ov8820_data;
 
@@ -762,7 +763,7 @@ static int ioctl_dev_init(struct v4l2_int_device *s)
 	ov8820_data.mclk = tgt_xclk;
 
 	pr_debug("   Setting mclk to %d MHz\n", tgt_xclk / 1000000);
-	set_mclk_rate(&ov8820_data.mclk, ov8820_data.csi);
+	set_mclk_rate(&ov8820_data.mclk, ov8820_data.mclk_source);
 
 	/* Default camera frame rate is set in probe */
 	tgt_fps = sensor->streamcap.timeperframe.denominator /
@@ -871,6 +872,7 @@ static int ov8820_probe(struct i2c_client *client,
 	memset(&ov8820_data, 0, sizeof(ov8820_data));
 	ov8820_data.mclk = 24000000; /* 6 - 54 MHz, typical 24MHz */
 	ov8820_data.mclk = plat_data->mclk;
+	ov8820_data.mclk_source = plat_data->mclk_source;
 	ov8820_data.csi = plat_data->csi;
 
 	ov8820_data.i2c_client = client;
diff --git a/drivers/media/video/mxc/output/mxc_vout.c b/drivers/media/video/mxc/output/mxc_vout.c
index aebe57fda0c5..e3e9483ac70c 100644
--- a/drivers/media/video/mxc/output/mxc_vout.c
+++ b/drivers/media/video/mxc/output/mxc_vout.c
@@ -1675,16 +1675,18 @@ static void release_disp_output(struct mxc_vout_output *vout)
 	set_window_position(vout, &pos);
 
 	if (get_ipu_channel(fbi) == MEM_BG_SYNC) {
-		console_lock();
-		fbi->fix.smem_start = vout->fb_smem_start;
-		fbi->fix.smem_len = vout->fb_smem_len;
-		vout->fb_var.activate |= FB_ACTIVATE_FORCE;
-		fbi->flags |= FBINFO_MISC_USEREVENT;
-		ret = fb_set_var(fbi, &vout->fb_var);
-		fbi->flags &= ~FBINFO_MISC_USEREVENT;
-		console_unlock();
-		if (ret < 0)
-			v4l2_err(vout->vfd->v4l2_dev, "ERR: fb_set_var.\n");
+		if ((vout->fb_smem_len != 0) && (vout->fb_smem_start != 0)) {
+			console_lock();
+			fbi->fix.smem_start = vout->fb_smem_start;
+			fbi->fix.smem_len = vout->fb_smem_len;
+			vout->fb_var.activate |= FB_ACTIVATE_FORCE;
+			fbi->flags |= FBINFO_MISC_USEREVENT;
+			ret = fb_set_var(fbi, &vout->fb_var);
+			fbi->flags &= ~FBINFO_MISC_USEREVENT;
+			console_unlock();
+			if (ret < 0)
+				v4l2_err(vout->vfd->v4l2_dev, "ERR: fb_set_var.\n");
+		}
 		console_lock();
 		fbi->flags |= FBINFO_MISC_USEREVENT;
 		fb_blank(fbi, FB_BLANK_UNBLANK);
diff --git a/drivers/mfd/pfuze-core.c b/drivers/mfd/pfuze-core.c
index 96b62924adec..9c7a988cf89b 100644
--- a/drivers/mfd/pfuze-core.c
+++ b/drivers/mfd/pfuze-core.c
@@ -501,9 +501,10 @@ static int pfuze_probe(struct i2c_client *client,
 		if (ret)
 			goto err_mask;
 	}
-	ret = request_threaded_irq(client->irq, NULL, pfuze_irq_thread,
-				   IRQF_ONESHOT | IRQF_TRIGGER_LOW, "pfuze",
-				   mc_pfuze);
+	if (client->irq)
+		ret = request_threaded_irq(client->irq, NULL, pfuze_irq_thread,
+					IRQF_ONESHOT | IRQF_TRIGGER_LOW, "pfuze",
+					mc_pfuze);
 	if (ret) {
 	      err_mask:
 	      err_revision:
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index 17773740d014..d20fc54eebf0 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -108,6 +108,8 @@ static u32 esdhc_readl_le(struct sdhci_host *host, int reg)
 {
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct pltfm_imx_data *imx_data = pltfm_host->priv;
+	struct esdhc_platform_data *boarddata
+				= host->mmc->parent->platform_data;
 
 	/* fake CARD_PRESENT flag on mx25/35 */
 	u32 val = readl(host->ioaddr + reg);
@@ -127,18 +129,18 @@ static u32 esdhc_readl_le(struct sdhci_host *host, int reg)
 		val |= (fsl_prss & 0x00800000) << 1;
 	}
 
-	if (unlikely((reg == SDHCI_PRESENT_STATE)
-			&& (imx_data->flags & ESDHC_FLAG_GPIO_FOR_CD_WP))) {
-		struct esdhc_platform_data *boarddata =
-				host->mmc->parent->platform_data;
-
-		if (boarddata && gpio_is_valid(boarddata->cd_gpio)
-				&& gpio_get_value(boarddata->cd_gpio))
-			/* no card, if a valid gpio says so... */
-			val &= ~SDHCI_CARD_PRESENT;
-		else
-			/* ... in all other cases assume card is present */
+	if (unlikely(reg == SDHCI_PRESENT_STATE)) {
+		if (boarddata && boarddata->always_present)
 			val |= SDHCI_CARD_PRESENT;
+		else if	(imx_data->flags & ESDHC_FLAG_GPIO_FOR_CD_WP) {
+			if (boarddata && gpio_is_valid(boarddata->cd_gpio)
+					&& gpio_get_value(boarddata->cd_gpio))
+				/* no card, if a valid gpio says so */
+				val &= ~SDHCI_CARD_PRESENT;
+			else
+				/* in all other cases assume card is present */
+				val |= SDHCI_CARD_PRESENT;
+		}
 	}
 
 	if (reg == SDHCI_INT_STATUS && cpu_is_mx6()
@@ -756,7 +758,7 @@ static int esdhc_pltfm_init(struct sdhci_host *host, struct sdhci_pltfm_data *pd
 	if (boarddata) {
 		/* Device is always present, e.x, populated emmc device */
 		if (boarddata->always_present) {
-			imx_data->flags |= ESDHC_FLAG_GPIO_FOR_CD_WP;
+			/* remove BROKEN_CD to disable card polling */
 			host->quirks &= ~SDHCI_QUIRK_BROKEN_CARD_DETECTION;
 			if (host->clk_mgr_en)
 				clk_disable(pltfm_host->clk);
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 1aedbe436c25..2f200b8aa49b 100755
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -35,6 +35,11 @@
 #define DBG(f, x...) \
 	pr_debug(DRIVER_NAME " [%s()]: " f, __func__,## x)
 
+#if defined(CONFIG_LEDS_CLASS) || (defined(CONFIG_LEDS_CLASS_MODULE) && \
+	defined(CONFIG_MMC_SDHCI_MODULE))
+/* FIXME: temp workaround for calling clk_enable in an interrupt context */
+#define SDHCI_USE_LEDS_CLASS_BROKEN
+#endif
 
 #define MAX_TUNING_LOOP 40
 
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
index 1c273e4eb2b4..75c2e618b28a 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
@@ -152,7 +152,7 @@ static int enable_ddr_onfi(struct gpmi_nand_data *this)
 	struct mtd_info	 *mtd = &this->mtd;
 	int saved_chip_number = 0;
 	uint8_t device_feature[FEATURE_SIZE];
-	int mode = 0;/* there is 5 mode available, default is 0 */
+	int mode = 5;/* there is 5 mode available, default is 0 */
 
 	saved_chip_number = this->current_chip;
 	nand->select_chip(mtd, 0);
@@ -178,14 +178,16 @@ static int enable_ddr_onfi(struct gpmi_nand_data *this)
 	/* [3] about the clock, pay attention! */
 	nand->select_chip(mtd, saved_chip_number);
 	{
-		struct clk *pll1;
-		pll1 = clk_get(NULL, "pll1_main_clk");
-		if (IS_ERR(pll1)) {
-			printk(KERN_INFO "No PLL1 clock\n");
+		struct clk *enfc_clk;
+		enfc_clk = clk_get(NULL, "enfc_clk");
+		if (IS_ERR(enfc_clk)) {
+			printk(KERN_INFO "No enfc_clk clock\n");
 			return -EINVAL;
 		}
-		clk_set_parent(resources->clock, pll1);
-		clk_set_rate(resources->clock, 20000000);
+		clk_set_parent(resources->clock, enfc_clk);
+		clk_set_rate(enfc_clk, \
+				enfc_clk->round_rate(enfc_clk, 100000000));
+		clk_set_rate(resources->clock, 100000000);
 	}
 	nand->select_chip(mtd, 0);
 
@@ -258,18 +260,20 @@ static int enable_ddr_toggle(struct gpmi_nand_data *this)
 	/* [3] about the clock, pay attention! */
 	nand->select_chip(mtd, saved_chip_number);
 	{
-		struct clk *pll1;
+		struct clk *enfc_clk;
 		unsigned long rate;
 
-		pll1 = clk_get(NULL, "pll1_main_clk");
-		if (IS_ERR(pll1)) {
-			printk(KERN_INFO "No PLL1 clock\n");
+		enfc_clk = clk_get(NULL, "enfc_clk");
+		if (IS_ERR(enfc_clk)) {
+			printk(KERN_INFO "No enfc_clk clock\n");
 			return -EINVAL;
 		}
 
 		/* toggle nand : 133/66 MHz */
 		rate = 33000000;
-		clk_set_parent(resources->clock, pll1);
+		clk_set_parent(resources->clock, enfc_clk);
+		clk_set_rate(enfc_clk, \
+				enfc_clk->round_rate(enfc_clk, rate));
 		clk_set_rate(resources->clock, rate);
 	}
 	nand->select_chip(mtd, 0);
diff --git a/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.c b/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.c
index 25a893cd614c..c24439d1af1e 100644
--- a/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.c
+++ b/drivers/mxc/gpu-viv/arch/GC350/hal/kernel/gc_hal_kernel_hardware_vg.c
@@ -1476,7 +1476,7 @@ static gceSTATUS _CommandStall(
         gcmkERR_BREAK(gckOS_WaitSignal(
             command->os,
             command->powerStallSignal,
-            gcvINFINITE));
+            gcdGPU_TIMEOUT));
 
 
     }
@@ -1750,7 +1750,7 @@ gckVGHARDWARE_SetPowerManagementState(
     if (flag & (gcvPOWER_FLAG_INITIALIZE | gcvPOWER_FLAG_CLOCK_ON))
     {
         /* Turn on the power. */
-        gcmkONERROR(gckOS_SetGPUPower(os, gcvCORE_VG , gcvTRUE, gcvTRUE));
+        gcmkONERROR(gckOS_SetGPUPower(os, gcvCORE_VG, gcvTRUE, gcvTRUE));
 
         /* Mark clock and power as enabled. */
         Hardware->clockState = gcvTRUE;
diff --git a/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_context.c b/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_context.c
index 6ddbcee2bda4..a186bbe3386e 100644
--- a/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_context.c
+++ b/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_context.c
@@ -545,7 +545,8 @@ _InitializeContextBuffer(
     index += _State(Context, index, 0x00A38 >> 2, 0x00000000, 1, gcvFALSE, gcvFALSE);
     index += _State(Context, index, 0x00A3C >> 2, 0x00000000, 1, gcvFALSE, gcvFALSE);
     index += _State(Context, index, 0x00A80 >> 2, 0x00000000, 1, gcvFALSE, gcvFALSE);
-    index += _State(Context, index, 0x00A84 >> 2, 0x00000000, 1, gcvFALSE, gcvFALSE);
+    index += _State(Context, index, 0x00A84 >> 2, 0x00000000, 1, gcvTRUE, gcvFALSE);
+    index += _State(Context, index, 0x00A8C >> 2, 0x00000000, 1, gcvFALSE, gcvFALSE);
 
     /* Setup states. */
     index += _State(Context, index, 0x00C00 >> 2, 0x00000000, 1, gcvTRUE, gcvFALSE);
diff --git a/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_hardware.c b/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_hardware.c
index 99b699d3a6c8..1578ff2b09c5 100644
--- a/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_hardware.c
+++ b/drivers/mxc/gpu-viv/arch/XAQ2/hal/kernel/gc_hal_kernel_hardware.c
@@ -217,16 +217,23 @@ _IdentifyHardware(
         }
     }
 
+    /* If new HZ is available, disable other early z modes. */
+    if (((((gctUINT32) (Identity->chipMinorFeatures3)) >> (0 ? 26:26) & ((gctUINT32) ((((1 ? 26:26) - (0 ? 26:26) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 26:26) - (0 ? 26:26) + 1)))))) == (0x1 & ((gctUINT32) ((((1 ? 26:26) - (0 ? 26:26) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 26:26) - (0 ? 26:26) + 1)))))))
+     || ((((gctUINT32) (Identity->chipMinorFeatures3)) >> (0 ? 8:8) & ((gctUINT32) ((((1 ? 8:8) - (0 ? 8:8) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:8) - (0 ? 8:8) + 1)))))) == (0x1 & ((gctUINT32) ((((1 ? 8:8) - (0 ? 8:8) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:8) - (0 ? 8:8) + 1))))))))
+    {
+        /* Disable EZ. */
+        Identity->chipFeatures
+            = ((((gctUINT32) (Identity->chipFeatures)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 16:16) - (0 ? 16:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 16:16) - (0 ? 16:16) + 1))))))) << (0 ? 16:16))) | (((gctUINT32) (0x1 & ((gctUINT32) ((((1 ? 16:16) - (0 ? 16:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 16:16) - (0 ? 16:16) + 1))))))) << (0 ? 16:16)));
+    }
+
 	/* Disable HZ when EZ is present for older chips. */
-    if (!((((gctUINT32) (Identity->chipMinorFeatures3)) >> (0 ? 8:8) & ((gctUINT32) ((((1 ? 8:8) - (0 ? 8:8) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:8) - (0 ? 8:8) + 1)))))) == (0x1 & ((gctUINT32) ((((1 ? 8:8) - (0 ? 8:8) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 8:8) - (0 ? 8:8) + 1)))))))
-        &&
-        !((((gctUINT32) (Identity->chipFeatures)) >> (0 ? 16:16) & ((gctUINT32) ((((1 ? 16:16) - (0 ? 16:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 16:16) - (0 ? 16:16) + 1)))))) == (0x1 & ((gctUINT32) ((((1 ? 16:16) - (0 ? 16:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 16:16) - (0 ? 16:16) + 1)))))))
-        )
+    else if (!((((gctUINT32) (Identity->chipFeatures)) >> (0 ? 16:16) & ((gctUINT32) ((((1 ? 16:16) - (0 ? 16:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 16:16) - (0 ? 16:16) + 1)))))) == (0x1 & ((gctUINT32) ((((1 ? 16:16) - (0 ? 16:16) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 16:16) - (0 ? 16:16) + 1))))))))
     {
         /* Disable HIERARCHICAL_Z. */
         Identity->chipMinorFeatures
             = ((((gctUINT32) (Identity->chipMinorFeatures)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 27:27) - (0 ? 27:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 27:27) - (0 ? 27:27) + 1))))))) << (0 ? 27:27))) | (((gctUINT32) (0x0 & ((gctUINT32) ((((1 ? 27:27) - (0 ? 27:27) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 27:27) - (0 ? 27:27) + 1))))))) << (0 ? 27:27)));
     }
+
     /* Disable rectangle primitive when chip is gc880_5_1_0_rc6*/
     if ((Identity->chipModel == gcv880) && (Identity->chipRevision == 0x5106))
     {
@@ -234,6 +241,7 @@ _IdentifyHardware(
         Identity->chipMinorFeatures2
             = ((((gctUINT32) (Identity->chipMinorFeatures2)) & ~(((gctUINT32) (((gctUINT32) ((((1 ? 5:5) - (0 ? 5:5) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 5:5) - (0 ? 5:5) + 1))))))) << (0 ? 5:5))) | (((gctUINT32) (0x0 & ((gctUINT32) ((((1 ? 5:5) - (0 ? 5:5) + 1) == 32) ? ~0 : (~(~0 << ((1 ? 5:5) - (0 ? 5:5) + 1))))))) << (0 ? 5:5)));
     }
+
     gcmkTRACE_ZONE(gcvLEVEL_INFO, gcvZONE_HARDWARE,
                    "Identity: chipFeatures=0x%08X",
                    Identity->chipFeatures);
@@ -494,7 +502,10 @@ gckHARDWARE_Construct(
 
     /* Enable the GPU. */
     gcmkONERROR(gckOS_SetGPUPower(Os, Core, gcvTRUE, gcvTRUE));
-    gcmkONERROR(gckOS_WriteRegisterEx(Os, Core, 0x00000, 0));
+    gcmkONERROR(gckOS_WriteRegisterEx(Os,
+                                      Core,
+                                      0x00000,
+                                      0x00000900));
 
     /* Allocate the gckHARDWARE object. */
     gcmkONERROR(gckOS_Allocate(Os,
@@ -609,7 +620,7 @@ OnError:
     if (hardware != gcvNULL)
     {
         /* Turn off the power. */
-        gcmkVERIFY_OK(gckOS_SetGPUPower(Os, hardware->core, gcvFALSE, gcvFALSE));
+        gcmkVERIFY_OK(gckOS_SetGPUPower(Os, Core, gcvFALSE, gcvFALSE));
 
         if (hardware->globalSemaphore != gcvNULL)
         {
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.c
index 187b9cc6d810..773a262562c4 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.c
@@ -289,7 +289,10 @@ OnError:
             if (kernel->hardware != gcvNULL)
             {
                 /* Turn off the power. */
-                gcmkVERIFY_OK(gckOS_SetGPUPower(kernel->hardware->os, kernel->hardware->core, gcvFALSE, gcvFALSE));
+                gcmkVERIFY_OK(gckOS_SetGPUPower(kernel->hardware->os,
+                                                kernel->hardware->core,
+                                                gcvFALSE,
+                                                gcvFALSE));
                 gcmkVERIFY_OK(gckHARDWARE_Destroy(kernel->hardware));
             }
         }
@@ -1037,7 +1040,7 @@ gckKERNEL_Dispatch(
         break;
 
     case gcvHAL_UNMAP_USER_MEMORY:
-        address = Interface->u.MapUserMemory.address;
+        address = Interface->u.UnmapUserMemory.address;
 
         /* Unmap user memory. */
         gcmkONERROR(
@@ -1169,9 +1172,10 @@ gckKERNEL_Dispatch(
 #if gcdREGISTER_ACCESS_FROM_USER
         {
             gceCHIPPOWERSTATE power;
+
+            gckOS_AcquireMutex(Kernel->os, Kernel->hardware->powerMutex, gcvINFINITE);
             gcmkONERROR(gckHARDWARE_QueryPowerManagementState(Kernel->hardware,
                                                               &power));
-
             if (power == gcvPOWER_ON)
             {
                 /* Read a register. */
@@ -1187,6 +1191,7 @@ gckKERNEL_Dispatch(
                 Interface->u.ReadRegisterData.data = 0;
                 status = gcvSTATUS_CHIP_NOT_READY;
             }
+            gcmkONERROR(gckOS_ReleaseMutex(Kernel->os, Kernel->hardware->powerMutex));
         }
 #else
         /* No access from user land to read registers. */
@@ -1197,12 +1202,29 @@ gckKERNEL_Dispatch(
 
     case gcvHAL_WRITE_REGISTER:
 #if gcdREGISTER_ACCESS_FROM_USER
-        /* Write a register. */
-        gcmkONERROR(
-            gckOS_WriteRegisterEx(Kernel->os,
-                                  Kernel->core,
-                                  Interface->u.WriteRegisterData.address,
-                                  Interface->u.WriteRegisterData.data));
+        {
+            gceCHIPPOWERSTATE power;
+
+            gckOS_AcquireMutex(Kernel->os, Kernel->hardware->powerMutex, gcvINFINITE);
+            gcmkONERROR(gckHARDWARE_QueryPowerManagementState(Kernel->hardware,
+                                                                  &power));
+            if (power == gcvPOWER_ON)
+            {
+                /* Write a register. */
+                gcmkONERROR(
+                    gckOS_WriteRegisterEx(Kernel->os,
+                                          Kernel->core,
+                                          Interface->u.WriteRegisterData.address,
+                                          Interface->u.WriteRegisterData.data));
+            }
+            else
+            {
+                /* Chip is in power-state. */
+                Interface->u.WriteRegisterData.data = 0;
+                status = gcvSTATUS_CHIP_NOT_READY;
+            }
+            gcmkONERROR(gckOS_ReleaseMutex(Kernel->os, Kernel->hardware->powerMutex));
+        }
 #else
         /* No access from user land to write registers. */
         status = gcvSTATUS_NOT_SUPPORTED;
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.h b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.h
index bdc94bc088b3..a9697a12c2e2 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel.h
@@ -613,24 +613,24 @@ typedef union _gcuVIDMEM_NODE
         gctSIZE_T               pageCount;
 
         /* Used only when node is not contiguous */
-        gctPOINTER              pageTables[gcdCORE_COUNT];
+        gctPOINTER              pageTables[gcdGPU_COUNT];
         /* Pointer to gckKERNEL object who lock this. */
-        gckKERNEL               lockKernels[gcdCORE_COUNT];
+        gckKERNEL               lockKernels[gcdGPU_COUNT];
         /* Actual physical address */
-        gctUINT32               addresses[gcdCORE_COUNT];
+        gctUINT32               addresses[gcdGPU_COUNT];
 
         /* Mutex. */
         gctPOINTER              mutex;
 
         /* Locked counter. */
-        gctINT32                lockeds[gcdCORE_COUNT];
+        gctINT32                lockeds[gcdGPU_COUNT];
 
 #ifdef __QNXNTO__
         /* Single linked list of nodes. */
         gcuVIDMEM_NODE_PTR      next;
 
         /* Unlock pending flag. */
-        gctBOOL                 unlockPendings[gcdCORE_COUNT];
+        gctBOOL                 unlockPendings[gcdGPU_COUNT];
 
         /* Free pending flag. */
         gctBOOL                 freePending;
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command.c
index 14ac175a1b2d..7a5e82eef3b4 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command.c
@@ -2480,8 +2480,6 @@ gckCOMMAND_Stall(
         gcmkONERROR(gckOS_Broadcast(
             os, hardware, gcvBROADCAST_GPU_STUCK
             ));
-
-        gcmkONERROR(gcvSTATUS_GPU_NOT_RESPONDING);
     }
 
     /* Delete the signal. */
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command_vg.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command_vg.c
index 3a29f023ff80..227ab683778e 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command_vg.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_command_vg.c
@@ -1236,12 +1236,6 @@ _TaskUnmapUserMemory(
     gcsBLOCK_TASK_ENTRY_PTR TaskHeader
     );
 
-static gceSTATUS
-_TaskUnmapMemory(
-    gckVGCOMMAND Command,
-    gcsBLOCK_TASK_ENTRY_PTR TaskHeader
-    );
-
 static gctTASKROUTINE _taskRoutine[] =
 {
     _TaskLink,                  /* gcvTASK_LINK                   */
@@ -1254,7 +1248,6 @@ static gctTASKROUTINE _taskRoutine[] =
     _TaskFreeVideoMemory,       /* gcvTASK_FREE_VIDEO_MEMORY      */
     _TaskFreeContiguousMemory,  /* gcvTASK_FREE_CONTIGUOUS_MEMORY */
     _TaskUnmapUserMemory,       /* gcvTASK_UNMAP_USER_MEMORY      */
-    _TaskUnmapMemory,           /* gcvTASK_UNMAP_MEMORY           */
 };
 
 static gceSTATUS
@@ -1676,38 +1669,6 @@ _TaskUnmapUserMemory(
     return status;
 }
 
-static gceSTATUS
-_TaskUnmapMemory(
-    gckVGCOMMAND Command,
-    gcsBLOCK_TASK_ENTRY_PTR TaskHeader
-    )
-{
-    gceSTATUS status;
-
-    do
-    {
-        /* Cast the task pointer. */
-        gcsTASK_UNMAP_MEMORY_PTR task
-            = (gcsTASK_UNMAP_MEMORY_PTR) TaskHeader->task;
-
-        /* Unmap memory. */
-        gcmkERR_BREAK(gckKERNEL_UnmapMemory(
-            Command->kernel->kernel, task->physical, task->bytes, task->logical
-            ));
-
-        /* Update the reference counter. */
-        TaskHeader->container->referenceCount -= 1;
-
-        /* Update the task pointer. */
-        TaskHeader->task = (gcsTASK_HEADER_PTR) (task + 1);
-    }
-    while (gcvFALSE);
-
-    /* Return status. */
-    return status;
-}
-
-
 /******************************************************************************\
 ************ Hardware Block Interrupt Handlers For Scheduled Events ************
 \******************************************************************************/
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_debug.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_debug.c
index 77ea27a6c31e..00decd9fb09d 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_debug.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_debug.c
@@ -1596,7 +1596,7 @@ _GetOutputBuffer(
 
 #if gcdTHREAD_BUFFERS > 1
     /* Get the current thread ID. */
-    gctUINT32 threadID = gcmkGETTHREADID();
+    gctUINT32 ThreadID = gcmkGETTHREADID();
 
     /* Locate the output buffer for the thread. */
     outputBuffer = _outputBufferHead;
@@ -1629,10 +1629,14 @@ _GetOutputBuffer(
 
         /* Reset the buffer. */
         outputBuffer->threadID   = ThreadID;
+#if gcdBUFFERED_OUTPUT
         outputBuffer->start      = 0;
         outputBuffer->index      = 0;
         outputBuffer->count      = 0;
+#endif
+#if gcdSHOW_LINE_NUMBER
         outputBuffer->lineNumber = 0;
+#endif
     }
 #else
     outputBuffer = _outputBufferHead;
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_event.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_event.c
index 4f8628c19946..43363b9e685b 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_event.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_event.c
@@ -203,20 +203,54 @@ _TryToIdleGPU(
 {
     gceSTATUS status;
     gctBOOL empty = gcvFALSE, idle = gcvFALSE;
+    gctUINT32 process, thread;
+    gctBOOL powerLocked = gcvFALSE;
+    gckHARDWARE hardware;
+
 
     gcmkHEADER_ARG("Event=0x%x", Event);
 
     /* Verify the arguments. */
     gcmkVERIFY_OBJECT(Event, gcvOBJ_EVENT);
 
+    /* Grab gckHARDWARE object. */
+    hardware = Event->kernel->hardware;
+    gcmkVERIFY_OBJECT(hardware, gcvOBJ_HARDWARE);
+
+
     /* Check whether the event queue is empty. */
     gcmkONERROR(gckEVENT_IsEmpty(Event, &empty));
 
     if (empty)
     {
+        status = gckOS_AcquireMutex(hardware->os, hardware->powerMutex, 0);
+        if (status == gcvSTATUS_TIMEOUT)
+        {
+            gcmkONERROR(gckOS_GetProcessID(&process));
+            gcmkONERROR(gckOS_GetThreadID(&thread));
+
+            /* Just return to prevent deadlock. */
+            if ((hardware->powerProcess != process)
+            ||  (hardware->powerThread  != thread))
+            {
+                gcmkFOOTER_NO();
+                return gcvSTATUS_OK;
+            }
+        }
+        else
+        {
+            powerLocked = gcvTRUE;
+        }
+
         /* Query whether the hardware is idle. */
         gcmkONERROR(gckHARDWARE_QueryIdle(Event->kernel->hardware, &idle));
 
+        if (powerLocked)
+        {
+            gcmkONERROR(gckOS_ReleaseMutex(hardware->os, hardware->powerMutex));
+            powerLocked = gcvFALSE;
+        }
+
         if (idle)
         {
             /* Inform the system of idle GPU. */
@@ -230,6 +264,13 @@ _TryToIdleGPU(
     return gcvSTATUS_OK;
 
 OnError:
+
+    if (powerLocked)
+    {
+        gcmkONERROR(gckOS_ReleaseMutex(hardware->os, hardware->powerMutex));
+        powerLocked = gcvFALSE;
+    }
+
     gcmkFOOTER();
     return status;
 }
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu.c
index deb658d98568..7062dd9a1b27 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_mmu.c
@@ -68,7 +68,7 @@ typedef struct _gcsSharedPageTable
     gckMMU          mmu;
 
     /* Hardwares which use this shared pagetable. */
-    gckHARDWARE     hardwares[gcdCORE_COUNT];
+    gckHARDWARE     hardwares[gcdGPU_COUNT];
 
     /* Number of cores use this shared pagetable. */
     gctUINT32       reference;
@@ -1201,7 +1201,7 @@ gckMMU_Enable(
         gcmkONERROR(_SetupDynamicSpace(Mmu));
 
 #if gcdSHARED_PAGETABLE
-        for(i = 0; i < gcdCORE_COUNT; i++)
+        for(i = 0; i < gcdGPU_COUNT; i++)
         {
             hardware = sharedPageTable->hardwares[i];
             if (hardware != gcvNULL)
@@ -1402,7 +1402,7 @@ gckMMU_Flush(
     gckHARDWARE hardware;
 #if gcdSHARED_PAGETABLE
     gctINT i;
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
 #if gcdENABLE_VG
         if (i == gcvCORE_VG)
diff --git a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_video_memory.c b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_video_memory.c
index 88e4d7705bd8..89a86a8486a7 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_video_memory.c
+++ b/drivers/mxc/gpu-viv/hal/kernel/gc_hal_kernel_video_memory.c
@@ -245,7 +245,7 @@ gckVIDMEM_ConstructVirtual(
     node->Virtual.contiguous    = Contiguous;
     node->Virtual.logical       = gcvNULL;
 
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         node->Virtual.lockeds[i]        = 0;
         node->Virtual.pageTables[i]     = gcvNULL;
@@ -259,7 +259,7 @@ gckVIDMEM_ConstructVirtual(
 #ifdef __QNXNTO__
     node->Virtual.next          = gcvNULL;
     node->Virtual.freePending   = gcvFALSE;
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         node->Virtual.unlockPendings[i] = gcvFALSE;
     }
@@ -366,7 +366,7 @@ gckVIDMEM_DestroyVirtual(
     /* Delete the mutex. */
     gcmkVERIFY_OK(gckOS_DeleteMutex(os, Node->Virtual.mutex));
 
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         if (Node->Virtual.pageTables[i] != gcvNULL)
         {
@@ -1392,7 +1392,7 @@ gckVIDMEM_Free(
 
     acquired = gcvTRUE;
 
-    for (i = 0, totalLocked = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0, totalLocked = 0; i < gcdGPU_COUNT; i++)
     {
         totalLocked += Node->Virtual.lockeds[i];
     }
@@ -1590,7 +1590,7 @@ _NeedVirtualMapping(
     gcmkVERIFY_ARGUMENT(Kernel != gcvNULL);
     gcmkVERIFY_ARGUMENT(Node != gcvNULL);
     gcmkVERIFY_ARGUMENT(NeedMapping != gcvNULL);
-    gcmkVERIFY_ARGUMENT(Core < gcdCORE_COUNT);
+    gcmkVERIFY_ARGUMENT(Core < gcdGPU_COUNT);
 
     if (Node->Virtual.contiguous)
     {
@@ -2052,7 +2052,7 @@ gckVIDMEM_Unlock(
 #endif
             }
 
-            for (i = 0, totalLocked = 0; i < gcdCORE_COUNT; i++)
+            for (i = 0, totalLocked = 0; i < gcdGPU_COUNT; i++)
             {
                 totalLocked += Node->Virtual.lockeds[i];
             }
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal.h
index 87b811d8dfaf..f986e4e4e009 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal.h
@@ -137,7 +137,7 @@ typedef enum _gceCORE
 }
 gceCORE;
 
-#define gcdCORE_COUNT               3
+#define gcdGPU_COUNT               3
 
 /*******************************************************************************
 **
@@ -1226,8 +1226,8 @@ gckOS_BroadcastCalibrateSpeed(
 **      gckOS Os
 **          Pointer to a gckOS object.ß
 **
-**      gceCORE Core
-**          Core type.
+**      gckCORE Core
+**          GPU whose power is set.
 **
 **      gctBOOL Clock
 **          gcvTRUE to turn on the clock, or gcvFALSE to turn off the clock.
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_base.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_base.h
index 5fafefbe058e..b1e6b488f0af 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_base.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_base.h
@@ -129,6 +129,7 @@ typedef struct _gcsTLS
 	gco3D						engine3D;
 #endif
 	gco2D						engine2D;
+    gctBOOL                     copied;
 }
 gcsTLS;
 
@@ -642,6 +643,9 @@ gcoOS_GetTLS(
     OUT gcsTLS_PTR * TLS
     );
 
+    /* Copy the TLS from a source thread. */
+    gceSTATUS gcoOS_CopyTLS(IN gcsTLS_PTR Source);
+
 /* Destroy the objects associated with the current thread. */
 void
 gcoOS_FreeThreadData(
@@ -2061,6 +2065,11 @@ gcoDUMP_Delete(
     IN gctUINT32 Address
     );
 
+/* Enable dump or not. */
+gceSTATUS
+gcoDUMP_SetDumpFlag(
+    IN gctBOOL DumpState
+    );
 
 /******************************************************************************\
 ******************************* gcsRECT Structure ******************************
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_driver_vg.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_driver_vg.h
index fe57968ba423..447663f964bd 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_driver_vg.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_driver_vg.h
@@ -109,8 +109,7 @@ typedef enum _gceTASK
     gcvTASK_UNLOCK_VIDEO_MEMORY,
     gcvTASK_FREE_VIDEO_MEMORY,
     gcvTASK_FREE_CONTIGUOUS_MEMORY,
-    gcvTASK_UNMAP_USER_MEMORY,
-    gcvTASK_UNMAP_MEMORY
+    gcvTASK_UNMAP_USER_MEMORY
 }
 gceTASK;
 
@@ -268,23 +267,6 @@ typedef struct _gcsTASK_UNMAP_USER_MEMORY
 }
 gcsTASK_UNMAP_USER_MEMORY;
 
-typedef struct _gcsTASK_UNMAP_MEMORY * gcsTASK_UNMAP_MEMORY_PTR;
-typedef struct _gcsTASK_UNMAP_MEMORY
-{
-    /* Task ID (gcvTASK_UNMAP_MEMORY). */
-    IN gceTASK                  id;
-
-    /* Physical memory address to unmap. */
-    IN gctPHYS_ADDR             physical;
-
-    /* Number of bytes in physical memory to unmap. */
-    IN gctSIZE_T                bytes;
-
-    /* Address of mapped memory to unmap. */
-    IN gctPOINTER               logical;
-}
-gcsTASK_UNMAP_MEMORY;
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform.h
index f4b7d2886e8e..b9bf7c8f916e 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_eglplatform.h
@@ -251,11 +251,63 @@ typedef struct __BITFIELDINFO{
 } BITFIELDINFO;
 
 #elif defined(LINUX) && defined(EGL_API_FB) && !defined(__APPLE__)
+
+#if defined(EGL_API_WL)
+/* Wayland platform. */
+
+#include <wayland-egl.h>
+
+#define WL_EGL_NUM_BACKBUFFERS 2
+
+struct wl_egl_buffer_info
+{
+   gctINT32 width;
+   gctINT32 height;
+   gctINT32 stride;
+   gctUINT32 physical;
+   gctPOINTER logical;
+   gceSURF_FORMAT format;
+   gcoSURF surface;
+};
+
+struct wl_egl_buffer
+{
+   struct wl_buffer* wl_buffer;
+   struct wl_egl_buffer_info info;
+};
+
+struct wl_egl_window_info
+{
+   gctUINT width;
+   gctUINT height;
+   gceSURF_FORMAT format;
+   gctUINT bpp;
+};
+
+struct wl_egl_window
+{
+/*   struct wl_egl_display *display;*/
+   struct wl_surface* surface;
+   struct wl_egl_window_info info;
+   struct wl_egl_buffer backbuffers[WL_EGL_NUM_BACKBUFFERS];
+   gctUINT current;
+   /*
+   int backbuffer;
+   int dx;
+   int dy;
+*/
+};
+
+
+typedef void*   HALNativeDisplayType;
+typedef void*   HALNativeWindowType;
+typedef void*   HALNativePixmapType;
+#else
 /* Linux platform for FBDEV. */
 typedef struct _FBDisplay * HALNativeDisplayType;
 typedef struct _FBWindow *  HALNativeWindowType;
 typedef struct _FBPixmap *  HALNativePixmapType;
-
+#endif
 #elif defined(__ANDROID__) || defined(ANDROID)
 
 struct egl_native_pixmap_t;
@@ -370,6 +422,8 @@ typedef struct _halDISPLAY_INFO
     ** if the address is not known for the specified display. */
     gctSIZE_T               physical;
 
+    gctBOOL                isCompositor;   /* true if compositor, false otherwise. */
+
 #ifndef __QNXNTO__
     /* 355_FB_MULTI_BUFFER */
     gctINT                      multiBuffer;
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_engine.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_engine.h
index f77cf5826a6a..7ad46da0b40e 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_engine.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_engine.h
@@ -1173,6 +1173,27 @@ gco3D_InvokeThreadWalker(
     IN gcsTHREAD_WALKER_INFO_PTR Info
     );
 
+#if gcdUSE_WCLIP_PATCH
+/* Set w clip and w plane limit value. */
+gceSTATUS
+gco3D_SetWClipEnable(
+	IN gco3D Engine,
+	IN gctBOOL Enable
+    );
+
+gceSTATUS
+gco3D_SetWPlaneLimitF(
+	IN gco3D Engine,
+	IN gctFLOAT Value
+    );
+
+gceSTATUS
+gco3D_SetWPlaneLimitX(
+	IN gco3D Engine,
+	IN gctFIXED_POINT Value
+    );
+#endif
+
 /*----------------------------------------------------------------------------*/
 /*-------------------------- gco3D Fragment Processor ------------------------*/
 
@@ -1437,6 +1458,7 @@ gceSTATUS
 gcoTEXTURE_AddMipMap(
     IN gcoTEXTURE Texture,
     IN gctINT Level,
+    IN gctINT imageFormat,
     IN gceSURF_FORMAT Format,
     IN gctUINT Width,
     IN gctUINT Height,
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_enum.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_enum.h
index d10686a259c3..18ddf5bff754 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_enum.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_enum.h
@@ -144,6 +144,7 @@ typedef enum _gceFEATURE
     gcvFEATURE_RS_YUV_TARGET,
     gcvFEATURE_2D_FC_SOURCE,
 	gcvFEATURE_PE_DITHER_FIX,
+    gcvFEATURE_2D_YUV_SEPARATE_STRIDE,
 }
 gceFEATURE;
 
@@ -263,6 +264,13 @@ typedef enum _gceSURF_ROTATION
 }
 gceSURF_ROTATION;
 
+typedef enum _gceMIPMAP_IMAGE_FORMAT
+{
+    gcvUNKNOWN_MIPMAP_IMAGE_FORMAT  = -2
+}
+gceMIPMAP_IMAGE_FORMAT;
+
+
 /* Surface formats. */
 typedef enum _gceSURF_FORMAT
 {
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_options.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_options.h
index 73b666a37570..35743c62f2a2 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_options.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_options.h
@@ -258,7 +258,7 @@
         virtual data.
 */
 #ifndef gcdMMU_SIZE
-#   define gcdMMU_SIZE                          (128 << 10)
+#   define gcdMMU_SIZE                          (2048 << 10)
 #endif
 
 /*
@@ -778,4 +778,8 @@
 #   define gcdHIGH_PRECISION_DELAY_ENABLE        1
 #endif
 
+#ifndef gcdUSE_WCLIP_PATCH
+#   define gcdUSE_WCLIP_PATCH                   0
+#endif
+
 #endif /* __gc_hal_options_h_ */
diff --git a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_version.h b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_version.h
index 132832c9224d..0bc1ffc46943 100644
--- a/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_version.h
+++ b/drivers/mxc/gpu-viv/hal/kernel/inc/gc_hal_version.h
@@ -28,9 +28,9 @@
 
 #define gcvVERSION_MINOR        6
 
-#define gcvVERSION_PATCH        7
+#define gcvVERSION_PATCH        8
 
-#define gcvVERSION_BUILD        1422
+#define gcvVERSION_BUILD        1443
 
 #define gcvVERSION_DATE      __DATE__
 
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.c b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.c
index df75a8b3c65c..305b1cc9acb6 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.c
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.c
@@ -404,7 +404,7 @@ gckGALDEVICE_Construct(
     device->requestedContiguousSize  = 0;
 
 
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         physical = device->requestedRegisterMemBases[i];
 
@@ -582,19 +582,19 @@ gckGALDEVICE_Construct(
     device->irqLines[gcvCORE_VG]    = IrqLineVG;
 
     /* Initialize the kernel thread semaphores. */
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         if (device->irqLines[i] != -1) sema_init(&device->semas[i], 0);
     }
 
     device->signal = Signal;
 
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         if (device->kernels[i] != gcvNULL) break;
     }
 
-    if (i == gcdCORE_COUNT) gcmkONERROR(gcvSTATUS_INVALID_ARGUMENT);
+    if (i == gcdGPU_COUNT) gcmkONERROR(gcvSTATUS_INVALID_ARGUMENT);
 
 #if gcdENABLE_VG
     if (i == gcvCORE_VG)
@@ -854,7 +854,7 @@ gckGALDEVICE_Destroy(
 
     if (Device != gcvNULL)
     {
-        for (i = 0; i < gcdCORE_COUNT; i++)
+        for (i = 0; i < gcdGPU_COUNT; i++)
         {
             if (Device->kernels[i] != gcvNULL)
             {
@@ -937,7 +937,7 @@ gckGALDEVICE_Destroy(
             }
         }
 
-        for (i = 0; i < gcdCORE_COUNT; i++)
+        for (i = 0; i < gcdGPU_COUNT; i++)
         {
             if (Device->registerBases[i] != gcvNULL)
             {
@@ -1428,7 +1428,7 @@ gckGALDEVICE_Stop_Threads(
 
     gcmkVERIFY_ARGUMENT(Device != NULL);
 
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         /* Stop the kernel threads. */
         if (Device->threadInitializeds[i])
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.h b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.h
index 69c985c90ff3..9e4f5b48e78f 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.h
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_device.h
@@ -38,7 +38,7 @@ typedef struct _gckGALDEVICE
 {
     /* Objects. */
     gckOS               os;
-    gckKERNEL           kernels[gcdCORE_COUNT];
+    gckKERNEL           kernels[gcdGPU_COUNT];
 
     /* Attributes. */
     gctSIZE_T           internalSize;
@@ -57,23 +57,23 @@ typedef struct _gckGALDEVICE
     gctPOINTER          contiguousMappedUser;
     gctSIZE_T           systemMemorySize;
     gctUINT32           systemMemoryBaseAddress;
-    gctPOINTER          registerBases[gcdCORE_COUNT];
-    gctSIZE_T           registerSizes[gcdCORE_COUNT];
+    gctPOINTER          registerBases[gcdGPU_COUNT];
+    gctSIZE_T           registerSizes[gcdGPU_COUNT];
     gctUINT32           baseAddress;
-    gctUINT32           requestedRegisterMemBases[gcdCORE_COUNT];
-    gctSIZE_T           requestedRegisterMemSizes[gcdCORE_COUNT];
+    gctUINT32           requestedRegisterMemBases[gcdGPU_COUNT];
+    gctSIZE_T           requestedRegisterMemSizes[gcdGPU_COUNT];
     gctUINT32           requestedContiguousBase;
     gctSIZE_T           requestedContiguousSize;
 
     /* IRQ management. */
-    gctINT              irqLines[gcdCORE_COUNT];
-    gctBOOL             isrInitializeds[gcdCORE_COUNT];
-    gctBOOL             dataReadys[gcdCORE_COUNT];
+    gctINT              irqLines[gcdGPU_COUNT];
+    gctBOOL             isrInitializeds[gcdGPU_COUNT];
+    gctBOOL             dataReadys[gcdGPU_COUNT];
 
     /* Thread management. */
-    struct task_struct  *threadCtxts[gcdCORE_COUNT];
-    struct semaphore    semas[gcdCORE_COUNT];
-    gctBOOL             threadInitializeds[gcdCORE_COUNT];
+    struct task_struct  *threadCtxts[gcdGPU_COUNT];
+    struct semaphore    semas[gcdGPU_COUNT];
+    gctBOOL             threadInitializeds[gcdGPU_COUNT];
     gctBOOL             killThread;
 
     /* Signal management. */
@@ -83,7 +83,7 @@ typedef struct _gckGALDEVICE
     gceCORE             coreMapping[8];
 
     /* States before suspend. */
-    gceCHIPPOWERSTATE   statesStored[gcdCORE_COUNT];
+    gceCHIPPOWERSTATE   statesStored[gcdGPU_COUNT];
 
     /* Clock management.*/
     struct clk         *clk_3d_core;
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c
index 0daf9a1cb149..c5ce248e77ba 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_driver.c
@@ -85,7 +85,7 @@ module_param(contiguousSize, ulong, 0644);
 static ulong contiguousBase = 0;
 module_param(contiguousBase, ulong, 0644);
 
-static ulong bankSize = 32 << 20;
+static ulong bankSize = 0;
 module_param(bankSize, ulong, 0644);
 
 static int fastClear = -1;
@@ -182,7 +182,7 @@ int drv_open(
     gcmkONERROR(gckOS_GetProcessID(&data->pidOpen));
 
     /* Attached the process. */
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         if (galDevice->kernels[i] != gcvNULL)
         {
@@ -225,7 +225,7 @@ OnError:
 
     if (attached)
     {
-        for (i = 0; i < gcdCORE_COUNT; i++)
+        for (i = 0; i < gcdGPU_COUNT; i++)
         {
             if (galDevice->kernels[i] != gcvNULL)
             {
@@ -304,7 +304,7 @@ int drv_release(
     }
 
     /* A process gets detached. */
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         if (galDevice->kernels[i] != gcvNULL)
         {
@@ -442,7 +442,7 @@ long drv_ioctl(
     if (iface.command == gcvHAL_CHIP_INFO)
     {
         count = 0;
-        for (i = 0; i < gcdCORE_COUNT; i++)
+        for (i = 0; i < gcdGPU_COUNT; i++)
         {
             if (device->kernels[i] != gcvNULL)
             {
@@ -959,7 +959,7 @@ static int __devinit gpu_suspend(struct platform_device *dev, pm_message_t state
 
     device = platform_get_drvdata(dev);
 
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         if (device->kernels[i] != gcvNULL)
         {
@@ -990,8 +990,6 @@ static int __devinit gpu_suspend(struct platform_device *dev, pm_message_t state
             {
                 status = gckHARDWARE_SetPowerManagementState(device->kernels[i]->hardware, gcvPOWER_OFF);
             }
-            /*gpu clock must be turned on before power down*/
-            gckOS_SetGPUPower(device->os, i, gcvTRUE, gcvFALSE);
             if (gcmIS_ERROR(status))
             {
                 return -1;
@@ -1012,7 +1010,7 @@ static int __devinit gpu_resume(struct platform_device *dev)
 
     device = platform_get_drvdata(dev);
 
-    for (i = 0; i < gcdCORE_COUNT; i++)
+    for (i = 0; i < gcdGPU_COUNT; i++)
     {
         if (device->kernels[i] != gcvNULL)
         {
diff --git a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_os.c b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_os.c
index cf0ccc2d4e2b..63ac50912833 100644
--- a/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_os.c
+++ b/drivers/mxc/gpu-viv/hal/os/linux/kernel/gc_hal_kernel_os.c
@@ -197,7 +197,6 @@ typedef struct _gcsSIGNAL
 
     /* ID. */
     gctUINT32 id;
-
 }
 gcsSIGNAL;
 
@@ -1150,6 +1149,52 @@ _DestroyIntegerId(
     return gcvSTATUS_OK;
 }
 
+static void
+_UnmapUserLogical(
+    IN gctINT Pid,
+    IN gctPOINTER Logical,
+    IN gctUINT32  Size
+)
+{
+    struct task_struct *task;
+    struct mm_struct *mm;
+
+    /* Get the task_struct of the task with stored pid. */
+    rcu_read_lock();
+
+    task = FIND_TASK_BY_PID(Pid);
+
+    if (task == gcvNULL)
+    {
+        rcu_read_unlock();
+        return;
+    }
+
+    /* Get the mm_struct. */
+    mm = get_task_mm(task);
+
+    rcu_read_unlock();
+
+    if (mm == gcvNULL)
+    {
+        return;
+    }
+
+    down_write(&mm->mmap_sem);
+    if (do_munmap(mm, (unsigned long)Logical, Size) < 0)
+    {
+        gcmkTRACE_ZONE(
+                gcvLEVEL_WARNING, gcvZONE_OS,
+                "%s(%d): do_munmap failed",
+                __FUNCTION__, __LINE__
+                );
+    }
+    up_write(&mm->mmap_sem);
+
+    /* Dereference. */
+    mmput(mm);
+}
+
 /*******************************************************************************
 **
 **  gckOS_Construct
@@ -1329,7 +1374,6 @@ gckOS_Destroy(
      */
 
     /* Destroy the mutex. */
-
     gcmkVERIFY_OK(gckOS_DeleteMutex(Os, Os->signalMutex));
 
     if (Os->heap != gcvNULL)
@@ -1898,7 +1942,6 @@ gckOS_UnmapMemoryEx(
 {
     PLINUX_MDL_MAP          mdlMap;
     PLINUX_MDL              mdl = (PLINUX_MDL)Physical;
-    struct task_struct *    task;
 
     gcmkHEADER_ARG("Os=0x%X Physical=0x%X Bytes=%lu Logical=0x%X PID=%d",
                    Os, Physical, Bytes, Logical, PID);
@@ -1924,24 +1967,7 @@ gckOS_UnmapMemoryEx(
             return gcvSTATUS_INVALID_ARGUMENT;
         }
 
-        /* Get the current pointer for the task with stored pid. */
-        task = FIND_TASK_BY_PID(mdlMap->pid);
-
-        if (task != gcvNULL && task->mm != gcvNULL)
-        {
-            down_write(&task->mm->mmap_sem);
-            do_munmap(task->mm, (unsigned long)Logical, mdl->numPages*PAGE_SIZE);
-            up_write(&task->mm->mmap_sem);
-        }
-        else
-        {
-            gcmkTRACE_ZONE(
-                gcvLEVEL_INFO, gcvZONE_OS,
-                "%s(%d): can't find the task with pid->%d. No unmapping",
-                __FUNCTION__, __LINE__,
-                mdlMap->pid
-                );
-        }
+        _UnmapUserLogical(PID, mdlMap->vmaAddr, mdl->numPages * PAGE_SIZE);
 
         gcmkVERIFY_OK(_DestroyMdlMap(mdl, mdlMap));
     }
@@ -2287,7 +2313,6 @@ gceSTATUS gckOS_FreeNonPagedMemory(
 {
     PLINUX_MDL mdl;
     PLINUX_MDL_MAP mdlMap;
-    struct task_struct * task;
 #ifdef NO_DMA_COHERENT
     unsigned size;
     gctPOINTER vaddr;
@@ -2350,27 +2375,7 @@ gceSTATUS gckOS_FreeNonPagedMemory(
     {
         if (mdlMap->vmaAddr != gcvNULL)
         {
-            /* Get the current pointer for the task with stored pid. */
-            task = FIND_TASK_BY_PID(mdlMap->pid);
-
-            if (task != gcvNULL && task->mm != gcvNULL)
-            {
-                down_write(&task->mm->mmap_sem);
-
-                if (do_munmap(task->mm,
-                              (unsigned long)mdlMap->vmaAddr,
-                              mdl->numPages * PAGE_SIZE) < 0)
-                {
-                    gcmkTRACE_ZONE(
-                        gcvLEVEL_WARNING, gcvZONE_OS,
-                        "%s(%d): do_munmap failed",
-                        __FUNCTION__, __LINE__
-                        );
-                }
-
-                up_write(&task->mm->mmap_sem);
-            }
-
+            _UnmapUserLogical(mdlMap->pid, mdlMap->vmaAddr, mdl->numPages * PAGE_SIZE);
             mdlMap->vmaAddr = gcvNULL;
         }
 
@@ -4687,7 +4692,6 @@ gckOS_UnlockPages(
 {
     PLINUX_MDL_MAP          mdlMap;
     PLINUX_MDL              mdl = (PLINUX_MDL)Physical;
-    struct task_struct *    task;
 
     gcmkHEADER_ARG("Os=0x%X Physical=0x%X Bytes=%u Logical=0x%X",
                    Os, Physical, Bytes, Logical);
@@ -4709,16 +4713,7 @@ gckOS_UnlockPages(
     {
         if ((mdlMap->vmaAddr != gcvNULL) && (_GetProcessID() == mdlMap->pid))
         {
-            /* Get the current pointer for the task with stored pid. */
-            task = FIND_TASK_BY_PID(mdlMap->pid);
-
-            if (task != gcvNULL && task->mm != gcvNULL)
-            {
-                down_write(&task->mm->mmap_sem);
-                do_munmap(task->mm, (unsigned long)mdlMap->vmaAddr, mdl->numPages * PAGE_SIZE);
-                up_write(&task->mm->mmap_sem);
-            }
-
+            _UnmapUserLogical(mdlMap->pid, mdlMap->vmaAddr, mdl->numPages * PAGE_SIZE);
             mdlMap->vmaAddr = gcvNULL;
         }
 
@@ -6914,8 +6909,8 @@ gckOS_GetThreadID(
 **      gckOS Os
 **          Pointer to a gckOS object.
 **
-**      gceCORE Core
-**          Core type.
+**      gckCORE Core
+**          GPU whose power is set.
 **
 **      gctBOOL Clock
 **          gcvTRUE to turn on the clock, or gcvFALSE to turn off the clock.
@@ -7597,7 +7592,6 @@ gckOS_MapSignal(
 {
     gceSTATUS status;
     gcsSIGNAL_PTR signal;
-
     gcmkHEADER_ARG("Os=0x%X Signal=0x%X Process=0x%X", Os, Signal, Process);
 
     gcmkVERIFY_ARGUMENT(Signal != gcvNULL);
@@ -7605,7 +7599,7 @@ gckOS_MapSignal(
 
     gcmkONERROR(_QueryIntegerId(&Os->signalDB, (gctUINT32)Signal, (gctPOINTER)&signal));
 
-    if (atomic_inc_return(&signal->ref) <= 1)
+    if(atomic_inc_return(&signal->ref) <= 1)
     {
         /* The previous value is 0, it has been deleted. */
         gcmkONERROR(gcvSTATUS_INVALID_ARGUMENT);
@@ -7674,6 +7668,7 @@ gckOS_CreateUserSignal(
     OUT gctINT * SignalID
     )
 {
+    /* Create a new signal. */
     return gckOS_CreateSignal(Os, ManualReset, (gctSIGNAL *) SignalID);
 }
 
diff --git a/drivers/mxc/ipu3/ipu_common.c b/drivers/mxc/ipu3/ipu_common.c
index 61687ad1411c..db22753e8276 100644
--- a/drivers/mxc/ipu3/ipu_common.c
+++ b/drivers/mxc/ipu3/ipu_common.c
@@ -40,6 +40,7 @@
 #include "ipu_regs.h"
 #include "ipu_param_mem.h"
 
+static DEFINE_MUTEX(ipu_clk_lock);
 static struct ipu_soc ipu_array[MXC_IPU_MAX_NUM];
 int g_ipu_hw_rev;
 
@@ -149,8 +150,8 @@ static int __devinit ipu_clk_setup_enable(struct ipu_soc *ipu,
 	}
 	dev_dbg(ipu->dev, "ipu_clk = %lu\n", clk_get_rate(ipu->ipu_clk));
 
-	ipu->pixel_clk[0] = ipu_pixel_clk[0];
-	ipu->pixel_clk[1] = ipu_pixel_clk[1];
+	ipu->pixel_clk[0] = ipu_pixel_clk[pdev->id][0];
+	ipu->pixel_clk[1] = ipu_pixel_clk[pdev->id][1];
 
 	ipu_lookups[pdev->id][0].clk = &ipu->pixel_clk[0];
 	ipu_lookups[pdev->id][1].clk = &ipu->pixel_clk[1];
@@ -371,14 +372,24 @@ void _ipu_unlock(struct ipu_soc *ipu)
 
 void _ipu_get(struct ipu_soc *ipu)
 {
-	if (atomic_inc_return(&ipu->ipu_use_count) == 1)
-		clk_enable(ipu->ipu_clk);
+	int ret;
+
+	if (in_interrupt())
+		return;
+	mutex_lock(&ipu_clk_lock);
+	ret = clk_enable(ipu->ipu_clk);
+	if (ret < 0)
+		BUG();
+	mutex_unlock(&ipu_clk_lock);
 }
 
 void _ipu_put(struct ipu_soc *ipu)
 {
-	if (atomic_dec_return(&ipu->ipu_use_count) == 0)
-		clk_disable(ipu->ipu_clk);
+	if (in_interrupt())
+		return;
+	mutex_lock(&ipu_clk_lock);
+	clk_disable(ipu->ipu_clk);
+	mutex_unlock(&ipu_clk_lock);
 }
 
 /*!
@@ -406,7 +417,6 @@ static int __devinit ipu_probe(struct platform_device *pdev)
 
 	spin_lock_init(&ipu->spin_lock);
 	mutex_init(&ipu->mutex_lock);
-	atomic_set(&ipu->ipu_use_count, 0);
 
 	g_ipu_hw_rev = plat_data->rev;
 
@@ -2683,10 +2693,13 @@ EXPORT_SYMBOL(ipu_clear_irq);
 bool ipu_get_irq_status(struct ipu_soc *ipu, uint32_t irq)
 {
 	uint32_t reg;
+	unsigned long lock_flags;
 
 	_ipu_get(ipu);
 
+	spin_lock_irqsave(&ipu->spin_lock, lock_flags);
 	reg = ipu_cm_read(ipu, IPUIRQ_2_STATREG(irq));
+	spin_unlock_irqrestore(&ipu->spin_lock, lock_flags);
 
 	_ipu_put(ipu);
 
@@ -2722,6 +2735,7 @@ int ipu_request_irq(struct ipu_soc *ipu, uint32_t irq,
 		    irqreturn_t(*handler) (int, void *),
 		    uint32_t irq_flags, const char *devname, void *dev_id)
 {
+	uint32_t reg;
 	unsigned long lock_flags;
 
 	BUG_ON(irq >= IPU_IRQ_COUNT);
@@ -2744,13 +2758,15 @@ int ipu_request_irq(struct ipu_soc *ipu, uint32_t irq,
 
 	/* clear irq stat for previous use */
 	ipu_cm_write(ipu, IPUIRQ_2_MASK(irq), IPUIRQ_2_STATREG(irq));
+	/* enable the interrupt */
+	reg = ipu_cm_read(ipu, IPUIRQ_2_CTRLREG(irq));
+	reg |= IPUIRQ_2_MASK(irq);
+	ipu_cm_write(ipu, reg, IPUIRQ_2_CTRLREG(irq));
 
 	spin_unlock_irqrestore(&ipu->spin_lock, lock_flags);
 
 	_ipu_put(ipu);
 
-	ipu_enable_irq(ipu, irq);	/* enable the interrupt */
-
 	return 0;
 }
 EXPORT_SYMBOL(ipu_request_irq);
@@ -2769,14 +2785,23 @@ EXPORT_SYMBOL(ipu_request_irq);
  */
 void ipu_free_irq(struct ipu_soc *ipu, uint32_t irq, void *dev_id)
 {
+	uint32_t reg;
 	unsigned long lock_flags;
 
-	ipu_disable_irq(ipu, irq);	/* disable the interrupt */
+	_ipu_get(ipu);
 
 	spin_lock_irqsave(&ipu->spin_lock, lock_flags);
+
+	/* disable the interrupt */
+	reg = ipu_cm_read(ipu, IPUIRQ_2_CTRLREG(irq));
+	reg &= ~IPUIRQ_2_MASK(irq);
+	ipu_cm_write(ipu, reg, IPUIRQ_2_CTRLREG(irq));
 	if (ipu->irq_list[irq].dev_id == dev_id)
 		ipu->irq_list[irq].handler = NULL;
+
 	spin_unlock_irqrestore(&ipu->spin_lock, lock_flags);
+
+	_ipu_put(ipu);
 }
 EXPORT_SYMBOL(ipu_free_irq);
 
@@ -2855,11 +2880,11 @@ uint32_t ipu_channel_status(struct ipu_soc *ipu, ipu_channel_t channel)
 {
 	uint32_t dma_status;
 
-	_ipu_lock(ipu);
 	_ipu_get(ipu);
+	_ipu_lock(ipu);
 	dma_status = ipu_is_channel_busy(ipu, channel);
-	_ipu_put(ipu);
 	_ipu_unlock(ipu);
+	_ipu_put(ipu);
 
 	dev_dbg(ipu->dev, "%s, dma_status:%d.\n", __func__, dma_status);
 
@@ -2979,13 +3004,16 @@ bool ipu_pixel_format_has_alpha(uint32_t fmt)
 	return false;
 }
 
-static int ipu_suspend_noirq(struct device *dev)
+static int ipu_suspend(struct platform_device *pdev, pm_message_t state)
 {
-	struct platform_device *pdev = to_platform_device(dev);
 	struct imx_ipuv3_platform_data *plat_data = pdev->dev.platform_data;
 	struct ipu_soc *ipu = platform_get_drvdata(pdev);
+	int i;
 
-	if (atomic_read(&ipu->ipu_use_count)) {
+	mutex_lock(&ipu_clk_lock);
+	ipu->ipu_use_count = clk_get_usecount(ipu->ipu_clk);
+	dev_dbg(ipu->dev, "%s, ipu_use_cnt:%d\n", __func__, ipu->ipu_use_count);
+	if (ipu->ipu_use_count) {
 		/* save and disable enabled channels*/
 		ipu->idma_enable_reg[0] = ipu_idmac_read(ipu, IDMAC_CHA_EN(0));
 		ipu->idma_enable_reg[1] = ipu_idmac_read(ipu, IDMAC_CHA_EN(32));
@@ -3009,7 +3037,7 @@ static int ipu_suspend_noirq(struct device *dev)
 			msleep(2);
 			time += 2;
 			if (time >= timeout)
-				return -1;
+				goto err;
 		}
 		ipu_idmac_write(ipu, 0, IDMAC_CHA_EN(0));
 		ipu_idmac_write(ipu, 0, IDMAC_CHA_EN(32));
@@ -3051,26 +3079,34 @@ static int ipu_suspend_noirq(struct device *dev)
 		ipu->buf_ready_reg[8] = ipu_cm_read(ipu, IPU_CHA_BUF2_RDY(0));
 		ipu->buf_ready_reg[9] = ipu_cm_read(ipu, IPU_CHA_BUF2_RDY(32));
 
-		clk_disable(ipu->ipu_clk);
+		for (i = 0; i < ipu->ipu_use_count; i++)
+			clk_disable(ipu->ipu_clk);
 	}
+	mutex_unlock(&ipu_clk_lock);
 
 	if (plat_data->pg)
 		plat_data->pg(1);
 
 	return 0;
+
+err:
+	mutex_unlock(&ipu_clk_lock);
+	return -1;
 }
 
-static int ipu_resume_noirq(struct device *dev)
+static int ipu_resume(struct platform_device *pdev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
 	struct imx_ipuv3_platform_data *plat_data = pdev->dev.platform_data;
 	struct ipu_soc *ipu = platform_get_drvdata(pdev);
+	int i;
 
 	if (plat_data->pg)
 		plat_data->pg(0);
 
-	if (atomic_read(&ipu->ipu_use_count)) {
-		clk_enable(ipu->ipu_clk);
+	dev_dbg(ipu->dev, "%s, ipu_use_cnt:%d\n", __func__, ipu->ipu_use_count);
+	if (ipu->ipu_use_count) {
+		for (i = 0; i < ipu->ipu_use_count; i++)
+			_ipu_get(ipu);
 
 		/* restore buf ready regs */
 		ipu_cm_write(ipu, ipu->buf_ready_reg[0], IPU_CHA_BUF0_RDY(0));
@@ -3122,20 +3158,16 @@ static int ipu_resume_noirq(struct device *dev)
 	return 0;
 }
 
-static const struct dev_pm_ops mxcipu_pm_ops = {
-	.suspend_noirq = ipu_suspend_noirq,
-	.resume_noirq = ipu_resume_noirq,
-};
-
 /*!
  * This structure contains pointers to the power management callback functions.
  */
 static struct platform_driver mxcipu_driver = {
 	.driver = {
 		   .name = "imx-ipuv3",
-		   .pm = &mxcipu_pm_ops,
 		   },
 	.probe = ipu_probe,
+	.suspend = ipu_suspend,
+	.resume = ipu_resume,
 	.remove = ipu_remove,
 };
 
diff --git a/drivers/mxc/ipu3/ipu_device.c b/drivers/mxc/ipu3/ipu_device.c
index 6810cea8f503..8de791bbd118 100644
--- a/drivers/mxc/ipu3/ipu_device.c
+++ b/drivers/mxc/ipu3/ipu_device.c
@@ -351,6 +351,7 @@ static DECLARE_WAIT_QUEUE_HEAD(thread_waitq);
 static DECLARE_WAIT_QUEUE_HEAD(res_waitq);
 static atomic_t req_cnt;
 static int major;
+static int max_ipu_no;
 static int thread_id;
 static atomic_t frame_no;
 static struct class *ipu_class;
@@ -1147,7 +1148,7 @@ static int _get_vdoa_ipu_res(struct ipu_task_entry *t)
 		}
 	}
 
-	for (i = 0; i < MXC_IPU_MAX_NUM; i++) {
+	for (i = 0; i < max_ipu_no; i++) {
 		ipu = ipu_get_soc(i);
 		if (IS_ERR(ipu))
 			BUG();
@@ -1176,7 +1177,7 @@ static int _get_vdoa_ipu_res(struct ipu_task_entry *t)
 	if (found_ipu)
 		goto next;
 
-	for (i = 0; i < MXC_IPU_MAX_NUM; i++) {
+	for (i = 0; i < max_ipu_no; i++) {
 		ipu = ipu_get_soc(i);
 		if (IS_ERR(ipu))
 			BUG();
@@ -1704,7 +1705,7 @@ static int init_tiled_buf(struct ipu_soc *ipu, struct ipu_task_entry *t,
 		return -EINVAL;
 	else if (param.band_mode)
 		param.band_lines = (1 << t->set.band_lines);
-	for (i = 0; i < MXC_IPU_MAX_NUM; i++) {
+	for (i = 0; i < max_ipu_no; i++) {
 		ipu_idx = ipu_get_soc(i);
 		if (!IS_ERR(ipu_idx) && ipu_idx == ipu)
 			break;
@@ -1712,7 +1713,7 @@ static int init_tiled_buf(struct ipu_soc *ipu, struct ipu_task_entry *t,
 	if (t->set.task & VDOA_ONLY)
 		/* dummy, didn't need ipu res */
 		i = 0;
-	if (MXC_IPU_MAX_NUM == i) {
+	if (max_ipu_no == i) {
 		dev_err(t->dev, "ERR:[0x%p] get ipu num\n", t);
 		return -EINVAL;
 	}
@@ -1749,7 +1750,9 @@ static int init_tiled_buf(struct ipu_soc *ipu, struct ipu_task_entry *t,
 			return -EINVAL;
 		}
 	}
-	vdoa_setup(t->vdoa_handle, &param);
+	ret = vdoa_setup(t->vdoa_handle, &param);
+	if (ret)
+		goto done;
 	vdoa_get_output_buf(t->vdoa_handle, &buf);
 	if (t->set.task & VDOA_ONLY)
 		goto done;
@@ -2883,7 +2886,7 @@ static void wait_split_task_complete(struct ipu_task_entry *parent,
 out:
 	if (ret == -ETIMEDOUT) {
 		/* debug */
-		for (k = 0; k < MXC_IPU_MAX_NUM; k++) {
+		for (k = 0; k < max_ipu_no; k++) {
 			ipu = ipu_get_soc(k);
 			if (IS_ERR(ipu)) {
 				BUG();
@@ -3407,6 +3410,7 @@ int register_ipu_device(struct ipu_soc *ipu, int id)
 
 		mutex_init(&ipu_ch_tbl.lock);
 	}
+	max_ipu_no = ++id;
 	ipu->rot_dma[0].size = 0;
 	ipu->rot_dma[1].size = 0;
 
diff --git a/drivers/mxc/ipu3/ipu_disp.c b/drivers/mxc/ipu3/ipu_disp.c
index d2fa65dd774e..4a774396ee99 100644
--- a/drivers/mxc/ipu3/ipu_disp.c
+++ b/drivers/mxc/ipu3/ipu_disp.c
@@ -174,44 +174,68 @@ static int _ipu_pixel_clk_set_parent(struct clk *clk, struct clk *parent)
 #else
 #define __INIT_CLK_DEBUG(n)
 #endif
-struct clk ipu_pixel_clk[] = {
+struct clk ipu_pixel_clk[MXC_IPU_MAX_NUM][MXC_DI_NUM_PER_IPU] = {
 	{
-		__INIT_CLK_DEBUG(pixel_clk_0)
-			.id = 0,
-		.get_rate = _ipu_pixel_clk_get_rate,
-		.set_rate = _ipu_pixel_clk_set_rate,
-		.round_rate = _ipu_pixel_clk_round_rate,
-		.set_parent = _ipu_pixel_clk_set_parent,
-		.enable = _ipu_pixel_clk_enable,
-		.disable = _ipu_pixel_clk_disable,
+		{
+		 __INIT_CLK_DEBUG(ipu1_pixel_clk_0)
+		 .id = 0,
+		 .get_rate = _ipu_pixel_clk_get_rate,
+		 .set_rate = _ipu_pixel_clk_set_rate,
+		 .round_rate = _ipu_pixel_clk_round_rate,
+		 .set_parent = _ipu_pixel_clk_set_parent,
+		 .enable = _ipu_pixel_clk_enable,
+		 .disable = _ipu_pixel_clk_disable,
+		},
+		{
+		 __INIT_CLK_DEBUG(ipu1_pixel_clk_1)
+		 .id = 1,
+		 .get_rate = _ipu_pixel_clk_get_rate,
+		 .set_rate = _ipu_pixel_clk_set_rate,
+		 .round_rate = _ipu_pixel_clk_round_rate,
+		 .set_parent = _ipu_pixel_clk_set_parent,
+		 .enable = _ipu_pixel_clk_enable,
+		 .disable = _ipu_pixel_clk_disable,
+		},
 	},
 	{
-		__INIT_CLK_DEBUG(pixel_clk_1)
-			.id = 1,
-		.get_rate = _ipu_pixel_clk_get_rate,
-		.set_rate = _ipu_pixel_clk_set_rate,
-		.round_rate = _ipu_pixel_clk_round_rate,
-		.set_parent = _ipu_pixel_clk_set_parent,
-		.enable = _ipu_pixel_clk_enable,
-		.disable = _ipu_pixel_clk_disable,
+		{
+		 __INIT_CLK_DEBUG(ipu2_pixel_clk_0)
+		 .id = 0,
+		 .get_rate = _ipu_pixel_clk_get_rate,
+		 .set_rate = _ipu_pixel_clk_set_rate,
+		 .round_rate = _ipu_pixel_clk_round_rate,
+		 .set_parent = _ipu_pixel_clk_set_parent,
+		 .enable = _ipu_pixel_clk_enable,
+		 .disable = _ipu_pixel_clk_disable,
+		},
+		{
+		 __INIT_CLK_DEBUG(ipu2_pixel_clk_1)
+		 .id = 1,
+		 .get_rate = _ipu_pixel_clk_get_rate,
+		 .set_rate = _ipu_pixel_clk_set_rate,
+		 .round_rate = _ipu_pixel_clk_round_rate,
+		 .set_parent = _ipu_pixel_clk_set_parent,
+		 .enable = _ipu_pixel_clk_enable,
+		 .disable = _ipu_pixel_clk_disable,
+		},
 	},
 };
 
-struct clk_lookup ipu_lookups[MXC_IPU_MAX_NUM][2] = {
+struct clk_lookup ipu_lookups[MXC_IPU_MAX_NUM][MXC_DI_NUM_PER_IPU] = {
 	{
 		{
-			.con_id = "pixel_clk_0",
+			.con_id = "ipu1_pixel_clk_0",
 		},
 		{
-			.con_id = "pixel_clk_1",
+			.con_id = "ipu1_pixel_clk_1",
 		},
 	},
 	{
 		{
-			.con_id = "pixel_clk_0",
+			.con_id = "ipu2_pixel_clk_0",
 		},
 		{
-			.con_id = "pixel_clk_1",
+			.con_id = "ipu2_pixel_clk_1",
 		},
 	},
 };
diff --git a/drivers/mxc/ipu3/ipu_ic.c b/drivers/mxc/ipu3/ipu_ic.c
index e21d6134c84a..84dcbb145f6f 100644
--- a/drivers/mxc/ipu3/ipu_ic.c
+++ b/drivers/mxc/ipu3/ipu_ic.c
@@ -841,7 +841,7 @@ static bool _calc_resize_coeffs(struct ipu_soc *ipu,
 	   where M = 2^13, SI - input size, SO - output size    */
 	*resizeCoeff = (8192L * (tempSize - 1)) / (outSize - 1);
 	if (*resizeCoeff >= 16384L) {
-		dev_err(ipu->dev, "Warning! Overflow on resize coeff.\n");
+		dev_dbg(ipu->dev, "Warning! Overflow on resize coeff.\n");
 		*resizeCoeff = 0x3FFF;
 	}
 
diff --git a/drivers/mxc/ipu3/ipu_prv.h b/drivers/mxc/ipu3/ipu_prv.h
index 526a2c0a36f0..7280dbaf00a8 100644
--- a/drivers/mxc/ipu3/ipu_prv.h
+++ b/drivers/mxc/ipu3/ipu_prv.h
@@ -20,16 +20,13 @@
 #include <linux/interrupt.h>
 #include <linux/fsl_devices.h>
 
-#ifdef CONFIG_MXC_IPU_V3H
-#define MXC_IPU_MAX_NUM	2
-#else
-#define MXC_IPU_MAX_NUM	1
-#endif
+#define MXC_IPU_MAX_NUM		2
+#define MXC_DI_NUM_PER_IPU	2
 
 /* Globals */
 extern int dmfc_type_setup;
-extern struct clk ipu_pixel_clk[];
-extern struct clk_lookup ipu_lookups[MXC_IPU_MAX_NUM][2];
+extern struct clk ipu_pixel_clk[MXC_IPU_MAX_NUM][MXC_DI_NUM_PER_IPU];
+extern struct clk_lookup ipu_lookups[MXC_IPU_MAX_NUM][MXC_DI_NUM_PER_IPU];
 
 #define IDMA_CHAN_INVALID	0xFF
 #define HIGH_RESOLUTION_WIDTH	1024
@@ -92,7 +89,7 @@ struct ipu_soc {
 	uint32_t channel_enable_mask;
 
 	/*use count*/
-	atomic_t ipu_use_count;
+	int ipu_use_count;
 	int dc_use_count;
 	int dp_use_count;
 	int dmfc_use_count;
diff --git a/drivers/mxc/ipu3/vdoa.c b/drivers/mxc/ipu3/vdoa.c
index 5800fc606f8c..f9cba78e27b4 100644
--- a/drivers/mxc/ipu3/vdoa.c
+++ b/drivers/mxc/ipu3/vdoa.c
@@ -25,10 +25,10 @@
 #include <linux/iram_alloc.h>
 
 #include "vdoa.h"
-/* FIXME: use cmdline to specify the iram size */
 /* 6band(3field* double buffer) * (width*2) * bandline(8)
 	= 6x1024x2x8 = 96k or 72k(1.5byte) */
-#define VDOA_IRAM_SIZE	(1024*96)
+#define MAX_VDOA_IRAM_SIZE	(1024*96)
+#define VDOA_IRAM_SIZE		(1024*72)
 
 #define VDOAC_BAND_HEIGHT_32LINES	(32)
 #define VDOAC_BAND_HEIGHT_16LINES	(16)
@@ -123,6 +123,7 @@ struct vdoa_info {
 };
 
 static struct vdoa_info *g_vdoa;
+static unsigned long iram_size;
 static DEFINE_MUTEX(vdoa_lock);
 
 static inline void vdoa_read_register(struct vdoa_info *vdoa,
@@ -148,15 +149,16 @@ static void dump_registers(struct vdoa_info *vdoa)
 		vdoa_read_register(vdoa, i, &data);
 }
 
-void vdoa_setup(vdoa_handle_t handle, struct vdoa_params *params)
+int vdoa_setup(vdoa_handle_t handle, struct vdoa_params *params)
 {
 	int	band_size;
+	int	total_band_size = 0;
 	int	ipu_stride;
 	u32	data;
 	struct vdoa_info *vdoa = (struct vdoa_info *)handle;
 
 	CHECK_NULL_PTR(vdoa);
-	CHECK_STATE(VDOA_GET | VDOA_GET_OBUF | VDOA_STOP, return);
+	CHECK_STATE(VDOA_GET | VDOA_GET_OBUF | VDOA_STOP, return -EINVAL);
 	if (VDOA_GET == vdoa->state) {
 		dev_dbg(vdoa->dev, "w:%d, h:%d.\n",
 			 params->width, params->height);
@@ -193,6 +195,12 @@ void vdoa_setup(vdoa_handle_t handle, struct vdoa_params *params)
 		band_size = ((params->width * 3) >> 1) *
 						params->band_lines;
 	if (params->interlaced) {
+		total_band_size = 6 * band_size; /* 3 frames*double buffer */
+		if (iram_size < total_band_size) {
+			dev_err(vdoa->dev, "iram_size:0x%lx is smaller than "
+				"request:0x%x!\n", iram_size, total_band_size);
+			return -EINVAL;
+		}
 		if (params->vfield_buf.prev_veba) {
 			if (params->band_mode) {
 				vdoa_write_register(vdoa, VDOAIEBA00,
@@ -265,6 +273,7 @@ void vdoa_setup(vdoa_handle_t handle, struct vdoa_params *params)
 					params->width * params->height);
 	}
 	vdoa->state = VDOA_SETUP;
+	return 0;
 }
 
 void vdoa_get_output_buf(vdoa_handle_t handle, struct vdoa_ipu_buf *buf)
@@ -330,7 +339,7 @@ void vdoa_stop(vdoa_handle_t handle)
 	struct vdoa_info *vdoa = (struct vdoa_info *)handle;
 
 	CHECK_NULL_PTR(vdoa);
-	CHECK_STATE(VDOA_START | VDOA_INIRQ, return);
+	CHECK_STATE(VDOA_GET | VDOA_START | VDOA_INIRQ, return);
 	vdoa->state = VDOA_STOP;
 
 	disable_irq(vdoa->irq);
@@ -394,6 +403,21 @@ static irqreturn_t vdoa_irq_handler(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
+/* IRAM Size in Kbytes, example:vdoa_iram_size=64, 64KBytes */
+static int __init vdoa_iram_size_setup(char *options)
+{
+	int ret;
+
+	ret = strict_strtoul(options, 0, &iram_size);
+	if (ret)
+		iram_size = 0;
+	else
+		iram_size *= SZ_1K;
+
+	return 1;
+}
+__setup("vdoa_iram_size=", vdoa_iram_size_setup);
+
 static int vdoa_probe(struct platform_device *pdev)
 {
 	int ret;
@@ -451,16 +475,16 @@ static int vdoa_probe(struct platform_device *pdev)
 		ret = PTR_ERR(vdoa->clk);
 		goto err_clk;
 	}
-
-	vdoa->iram_base = iram_alloc(VDOA_IRAM_SIZE, &vdoa->iram_paddr);
+	if ((iram_size == 0) || (iram_size > MAX_VDOA_IRAM_SIZE))
+		iram_size = VDOA_IRAM_SIZE;
+	vdoa->iram_base = iram_alloc(iram_size, &vdoa->iram_paddr);
 	if (!vdoa->iram_base) {
-		dev_err(dev, "failed to get iram memory:0x%x\n",
-				VDOA_IRAM_SIZE);
+		dev_err(dev, "failed to get iram memory:0x%lx\n", iram_size);
 		ret = -ENOMEM;
 		goto err_iram_alloc;
 	}
-	dev_dbg(dev, "iram_base:0x%p,iram_paddr:0x%lx,size:0x%x\n",
-		 vdoa->iram_base, vdoa->iram_paddr, VDOA_IRAM_SIZE);
+	dev_dbg(dev, "iram_base:0x%p,iram_paddr:0x%lx,size:0x%lx\n",
+		 vdoa->iram_base, vdoa->iram_paddr, iram_size);
 
 	vdoa->state = VDOA_INIT;
 	dev_set_drvdata(dev, vdoa);
@@ -491,7 +515,7 @@ static int __devexit vdoa_remove(struct platform_device *pdev)
 
 	clk_put(vdoa->clk);
 	clk_disable(vdoa->clk);
-	iram_free(vdoa->iram_paddr, VDOA_IRAM_SIZE);
+	iram_free(vdoa->iram_paddr, iram_size);
 	iounmap(vdoa->reg_base);
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
diff --git a/drivers/mxc/ipu3/vdoa.h b/drivers/mxc/ipu3/vdoa.h
index 82b0ee1f2c3c..23d72497a0a7 100644
--- a/drivers/mxc/ipu3/vdoa.h
+++ b/drivers/mxc/ipu3/vdoa.h
@@ -60,7 +60,7 @@ struct vdoa_ipu_buf {
 struct vdoa_info;
 typedef void *vdoa_handle_t;
 
-void vdoa_setup(vdoa_handle_t handle, struct vdoa_params *params);
+int vdoa_setup(vdoa_handle_t handle, struct vdoa_params *params);
 void vdoa_get_output_buf(vdoa_handle_t handle, struct vdoa_ipu_buf *buf);
 int  vdoa_start(vdoa_handle_t handle, int timeout_ms);
 void vdoa_stop(vdoa_handle_t handle);
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index 27dcf755debc..467b5355de89 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -195,6 +195,7 @@ struct flexcan_priv {
 	struct clk *clk;
 	struct flexcan_platform_data *pdata;
 	enum flexcan_ip_version version;
+	int id;
 };
 
 static struct can_bittiming_const flexcan_bittiming_const = {
@@ -580,7 +581,8 @@ static irqreturn_t flexcan_irq(int irq, void *dev_id)
 	if (reg_esr & FLEXCAN_ESR_WAK_INT) {
 		/* first clear stop request then wakeup irq status */
 		if (priv->version >= FLEXCAN_VER_10_0_12)
-			mxc_iomux_set_gpr_register(13, 28, 1, 0);
+			/* CAN1/CAN2_STOP_REQ bit 28/29 in group 13 */
+			mxc_iomux_set_gpr_register(13, 28 + priv->id, 1, 0);
 		writel(FLEXCAN_ESR_WAK_INT, &regs->esr);
 	}
 
@@ -960,6 +962,9 @@ static int __devinit flexcan_probe(struct platform_device *pdev)
 	resource_size_t mem_size;
 	int err, irq;
 
+	if (pdev->id < 0)
+		return -ENODEV;
+
 	clk = clk_get(&pdev->dev, NULL);
 	if (IS_ERR(clk)) {
 		dev_err(&pdev->dev, "no clock defined\n");
@@ -1007,6 +1012,7 @@ static int __devinit flexcan_probe(struct platform_device *pdev)
 	priv->base = base;
 	priv->dev = dev;
 	priv->clk = clk;
+	priv->id = pdev->id;
 	priv->pdata = pdev->dev.platform_data;
 	priv->version = pdev->id_entry->driver_data;
 
@@ -1074,7 +1080,8 @@ static int flexcan_suspend(struct platform_device *pdev, pm_message_t state)
 
 	/* enable stop request for wakeup */
 	if (priv->version >= FLEXCAN_VER_10_0_12)
-		mxc_iomux_set_gpr_register(13, 28, 1, 1);
+		/* CAN1/CAN2_STOP_REQ bit 28/29 in group 13 */
+		mxc_iomux_set_gpr_register(13, 28 + priv->id, 1, 1);
 
 	ret = irq_set_irq_wake(dev->irq, 1);
 	if (ret)
diff --git a/drivers/net/fec.c b/drivers/net/fec.c
index 6e710e11f1a4..acb2965ae77d 100755
--- a/drivers/net/fec.c
+++ b/drivers/net/fec.c
@@ -71,17 +71,14 @@
 #define FEC_QUIRK_SWAP_FRAME		(1 << 1)
 
 static struct platform_device_id fec_devtype[] = {
-#ifdef CONFIG_SOC_IMX6Q
 	{
-		.name = DRIVER_NAME,
+		.name = "enet",
 		.driver_data = FEC_QUIRK_ENET_MAC,
 	},
-#else
 	{
-		.name = DRIVER_NAME,
+		.name = "fec",
 		.driver_data = 0,
 	},
-#endif
 	{
 		.name = "imx28-fec",
 		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_SWAP_FRAME,
@@ -146,12 +143,11 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #define FEC_ENET_MII_CLK       ((uint)2500000)
 #define FEC_ENET_HOLD_TIME     ((uint)0x100)  /* 2 internal clock cycle*/
 
-#if defined(CONFIG_FEC_1588) && (defined(CONFIG_ARCH_MX28) || \
-				defined(CONFIG_ARCH_MX6))
-#define FEC_DEFAULT_IMASK (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII | \
-				FEC_ENET_TS_AVAIL | FEC_ENET_TS_TIMER)
-#else
 #define FEC_DEFAULT_IMASK (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII)
+#if defined(CONFIG_FEC_1588)
+#define FEC_1588_IMASK	  (FEC_ENET_TS_AVAIL | FEC_ENET_TS_TIMER)
+#else
+#define FEC_1588_IMASK	0
 #endif
 
 /* The FEC stores dest/src/type, data, and checksum for receive packets.
@@ -1032,7 +1028,7 @@ static int fec_enet_mii_probe(struct net_device *ndev)
 	}
 
 	/* mask with MAC supported features */
-	if (cpu_is_mx6())
+	if (cpu_is_mx6q() || cpu_is_mx6dl())
 		phy_dev->supported &= PHY_GBIT_FEATURES;
 	else
 		phy_dev->supported &= PHY_BASIC_FEATURES;
@@ -1091,7 +1087,7 @@ static int fec_enet_mii_init(struct platform_device *pdev)
 					(FEC_ENET_MII_CLK << 2)) << 1;
 
 	/* set hold time to 2 internal clock cycle */
-	if (cpu_is_mx6())
+	if (cpu_is_mx6q() || cpu_is_mx6dl())
 		fep->phy_speed |= FEC_ENET_HOLD_TIME;
 
 	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
@@ -1546,14 +1542,14 @@ fec_restart(struct net_device *dev, int duplex)
 	udelay(10);
 
 	/*
-	 * enet-mac reset will reset mac address registers too,
-	 * so need to reconfigure it.
+	/* if uboot don't set MAC address, get MAC address
+	 * from command line; if command line don't set MAC
+	 * address, get from OCOTP; otherwise, allocate random
+	 * address.
 	 */
-	if (id_entry->driver_data & FEC_QUIRK_ENET_MAC) {
-		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);
-	}
+	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);
 
 	/* Clear any outstanding interrupt. */
 	writel(0xffc00000, fep->hwp + FEC_IEVENT);
@@ -1623,23 +1619,24 @@ fec_restart(struct net_device *dev, int duplex)
 			val |= (1 << 9);
 
 		writel(val, fep->hwp + FEC_R_CNTRL);
+	}
 
-		if (fep->ptimer_present) {
-			/* Set Timer count */
-			ret = fec_ptp_start(fep->ptp_priv);
-			if (ret) {
-				fep->ptimer_present = 0;
-				reg = 0x0;
-			} else
+	if (fep->ptimer_present) {
+		/* Set Timer count */
+		ret = fec_ptp_start(fep->ptp_priv);
+		if (ret) {
+			fep->ptimer_present = 0;
+			reg = 0x0;
+		} else
 #if defined(CONFIG_SOC_IMX28) || defined(CONFIG_ARCH_MX6)
-				reg = 0x00000010;
+			reg = 0x00000010;
 #else
-				reg = 0x0;
+			reg = 0x0;
 #endif
 	} else
 		reg = 0x0;
 
-#ifdef FEC_MIIGSK_ENR
+	if (cpu_is_mx25() || cpu_is_mx53() || cpu_is_mx6sl()) {
 		if (fep->phy_interface == PHY_INTERFACE_MODE_RMII) {
 			/* disable the gasket and wait */
 			writel(0, fep->hwp + FEC_MIIGSK_ENR);
@@ -1654,8 +1651,14 @@ fec_restart(struct net_device *dev, int duplex)
 
 			/* re-enable the gasket */
 			writel(2, fep->hwp + FEC_MIIGSK_ENR);
+			udelay(10);
+			if (!(readl(fep->hwp + FEC_MIIGSK_ENR) & 4)) {
+				udelay(100);
+				if (!(readl(fep->hwp + FEC_MIIGSK_ENR) & 4))
+					dev_err(&fep->pdev->dev,
+						"switch to RMII failed!\n");
+			}
 		}
-#endif
 	}
 
 	/* ENET enable */
@@ -1668,7 +1671,7 @@ fec_restart(struct net_device *dev, int duplex)
 		fep->phy_dev->speed == SPEED_1000)
 		val |= (0x1 << 5);
 
-	if (cpu_is_mx6()) {
+	if (cpu_is_mx6q() || cpu_is_mx6dl()) {
 		/* enable endian swap */
 		val |= (0x1 << 8);
 		/* enable ENET store and forward mode */
@@ -1679,7 +1682,11 @@ fec_restart(struct net_device *dev, int duplex)
 	writel(0, fep->hwp + FEC_R_DES_ACTIVE);
 
 	/* Enable interrupts we wish to service */
-	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
+	if (cpu_is_mx6q() || cpu_is_mx6dl() || cpu_is_mx2() || cpu_is_mx3())
+		val = (FEC_1588_IMASK | FEC_DEFAULT_IMASK);
+	else
+		val = FEC_DEFAULT_IMASK;
+	writel(val, fep->hwp + FEC_IMASK);
 }
 
 static void
@@ -1699,7 +1706,7 @@ fec_stop(struct net_device *dev)
 	writel(1, fep->hwp + FEC_ECNTRL);
 	udelay(10);
 
-	if (cpu_is_mx6())
+	if (cpu_is_mx6q() || cpu_is_mx6dl())
 		/* FIXME: we have to enable enet to keep mii interrupt works. */
 		writel(2, fep->hwp + FEC_ECNTRL);
 
@@ -1707,6 +1714,8 @@ fec_stop(struct net_device *dev)
 	if (fep->ptimer_present)
 		fec_ptp_stop(fep->ptp_priv);
 	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
+	netif_stop_queue(dev);
+	fep->link = 0;
 }
 
 static int __devinit
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 8145ff85c8ab..6e53cce3c7fd 100755
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -219,6 +219,15 @@ config CHARGER_MAX8903
 	  pins based on the status of charger connections with interrupt
 	  handlers.
 
+config SABRESD_MAX8903
+        tristate "Sabresd Board Battery DC-DC Charger for USB and Adapter Power"
+        depends on GENERIC_HARDIRQS
+	depends on TOUCHSCREEN_MAX11801
+        help
+          Say Y to enable support for the MAX8903 DC-DC charger and sysfs on
+	  sabresd board.The driver supports controlling charger and battery
+	  based on the status of charger connections with interrupt handlers.
+
 config CHARGER_TWL4030
 	tristate "OMAP TWL4030 BCI charger driver"
 	depends on TWL4030_CORE
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index ad6493affec8..1f359ba8a03c 100755
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -37,3 +37,4 @@ obj-$(CONFIG_CHARGER_MAX8903)	+= max8903_charger.o
 obj-$(CONFIG_CHARGER_TWL4030)	+= twl4030_charger.o
 obj-$(CONFIG_CHARGER_GPIO)	+= gpio-charger.o
 obj-$(CONFIG_BATTERY_DA9052)	+= da9052-battery.o
+obj-$(CONFIG_SABRESD_MAX8903)   += sabresd_battery.o
diff --git a/drivers/power/max8903_battery.c b/drivers/power/max8903_battery.c
new file mode 100755
index 000000000000..84b5373cbb2f
--- /dev/null
+++ b/drivers/power/max8903_battery.c
@@ -0,0 +1,748 @@
+/*
+ * max8903_battery.c - Maxim 8903 USB/Adapter Charger Driver
+ *
+ * Copyright (C) 2011 Samsung Electronics
+ * Copyright (C) 2011-2012 Freescale Semiconductor, Inc.
+ * Based on max8903_charger.c
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/power_supply.h>
+#include <linux/platform_device.h>
+#include <linux/power/max8903_battery.h>
+#include <linux/sort.h>
+
+
+#define	BATTERY_UPDATE_INTERVAL	5 /*seconds*/
+#define LOW_VOLT_THRESHOLD	2800000
+#define HIGH_VOLT_THRESHOLD	4200000
+#define ADC_SAMPLE_COUNT	4
+
+struct max8903_data {
+	struct max8903_pdata *pdata;
+	struct device *dev;
+	struct power_supply psy;
+	bool fault;
+	bool usb_in;
+	bool ta_in;
+	bool chg_state;
+	struct delayed_work work;
+	unsigned int interval;
+	unsigned short thermal_raw;
+	int voltage_uV;
+	int current_uA;
+	int battery_status;
+	int charger_online;
+	int charger_voltage_uV;
+	int real_capacity;
+	int percent;
+	int old_percent;
+	struct power_supply bat;
+	struct mutex work_lock;
+};
+
+typedef struct {
+	u32 voltage;
+	u32 percent;
+} battery_capacity , *pbattery_capacity;
+
+static bool capacity_changed_flag;
+
+static battery_capacity chargingTable[] = {
+    {4148, 100},
+	{4126,	99},
+	{4123,	98},
+	{4120,	97},
+	{4105,	96},
+	{4090,	96},
+	{4075,	95},
+	{4060,	94},
+	{4045,	93},
+	{4030,	92},
+	{4015,  91},
+    {4000,  90},
+    {3900,  85},
+    {3790,  80},
+    {3760,  75},
+    {3730,  70},
+    {3700,  65},
+    {3680,  60},
+    {3660,  55},
+    {3640,  50},
+    {3600,  45},
+    {3550,  40},
+    {3510,  35},
+    {3450,  30},
+    {3310,  25},
+    {3240,  20},
+    {3180,  15},
+    {3030,  10},
+    {2820,  5},
+    {2800,  0},
+    {0,  0}
+};
+static battery_capacity dischargingTable[] = {
+    {4100, 100},
+	{4090,	99},
+	{4080,	98},
+	{4060,	97},
+	{4040,	96},
+	{3920,	96},
+	{3900,	95},
+	{3970,	94},
+	{3940,	93},
+	{3910,	92},
+	{3890,  91},
+    {3860,  90},
+    {3790,  85},
+    {3690,  80},
+    {3660,  75},
+    {3630,  70},
+    {3600,  65},
+    {3580,  60},
+    {3560,  55},
+    {3540,  50},
+    {3500,  45},
+    {3450,  40},
+    {3410,  35},
+    {3350,  30},
+    {3310,  25},
+    {3240,  20},
+    {3180,  15},
+    {3030,  10},
+    {2820,  5},
+    {2800,  0},
+    {0,  0}
+};
+
+u32 calibrate_battery_capability_percent(struct max8903_data *data)
+{
+    u8 i;
+    pbattery_capacity pTable;
+    u32 tableSize;
+    if (data->battery_status  == POWER_SUPPLY_STATUS_DISCHARGING) {
+		pTable = dischargingTable;
+		tableSize = sizeof(dischargingTable)/sizeof(dischargingTable[0]);
+    } else {
+		pTable = chargingTable;
+		tableSize = sizeof(chargingTable)/sizeof(chargingTable[0]);
+    }
+    for (i = 0; i < tableSize; i++) {
+		if (data->voltage_uV >= pTable[i].voltage)
+			return	pTable[i].percent;
+    }
+
+    return 0;
+}
+
+static enum power_supply_property max8903_charger_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_HEALTH,
+};
+
+static enum power_supply_property max8903_battery_props[] = {
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_ONLINE,
+	POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN,
+	POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN,
+};
+
+extern u32 max11801_read_adc(void);
+
+static void max8903_charger_update_status(struct max8903_data *data)
+{
+	if (data->usb_in || data->ta_in) {
+			data->charger_online = 1;
+		} else {
+			data->charger_online = 0;
+		}
+	if (data->charger_online == 0) {
+			data->battery_status = POWER_SUPPLY_STATUS_DISCHARGING;
+		} else {
+			if (data->pdata->chg) {
+					if (gpio_get_value(data->pdata->chg) == 0) {
+						data->battery_status = POWER_SUPPLY_STATUS_CHARGING;
+					} else if ((data->usb_in || data->ta_in) &&
+						gpio_get_value(data->pdata->chg) == 1) {
+						data->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+					}
+			}
+		}
+	pr_debug("chg: %d \n", gpio_get_value(data->pdata->chg));
+}
+static int cmp_func(const void *_a, const void *_b)
+{
+	const int *a = _a, *b = _b;
+
+	if (*a > *b)
+		return 1;
+	if (*a < *b)
+		return -1;
+	return 0;
+}
+u32 calibration_voltage(struct max8903_data *data)
+{
+	int volt[ADC_SAMPLE_COUNT];
+	u32 voltage_data;
+	int i;
+	for (i = 0; i < ADC_SAMPLE_COUNT; i++) {
+		if (data->charger_online == 0) {
+			/* ADC offset when battery is discharger*/
+			volt[i] = max11801_read_adc()-1880;
+			} else {
+			volt[i] = max11801_read_adc()-1960;
+			}
+	}
+	sort(volt, i, 4, cmp_func, NULL);
+	for (i = 0; i < ADC_SAMPLE_COUNT; i++)
+		pr_debug("volt_sorted[%2d]: %d\n", i, volt[i]);
+	/* get the average of second max/min of remained. */
+	voltage_data = (volt[1] + volt[i - 2]) / 2;
+	return voltage_data;
+}
+
+static void max8903_battery_update_status(struct max8903_data *data)
+{
+	static int counter;
+#if 0
+	data->voltage_uV = max11801_read_adc();
+#endif
+	mutex_lock(&data->work_lock);
+	data->voltage_uV = calibration_voltage(data);
+	data->percent = calibrate_battery_capability_percent(data);
+	if (data->percent != data->old_percent) {
+		data->old_percent = data->percent;
+		capacity_changed_flag = true;
+	}
+	if ((capacity_changed_flag == true) && (data->charger_online)) {
+		counter++;
+		if (counter > 2) {
+			counter = 0;
+			capacity_changed_flag = false;
+			power_supply_changed(&data->bat);
+		}
+	}
+	mutex_unlock(&data->work_lock);
+}
+
+static int max8903_battery_get_property(struct power_supply *bat,
+				       enum power_supply_property psp,
+				       union power_supply_propval *val)
+{
+	struct max8903_data *di = container_of(bat,
+			struct max8903_data, bat);
+	static unsigned long last;
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
+		if (di->pdata->chg) {
+			if (gpio_get_value(di->pdata->chg) == 0) {
+				val->intval = POWER_SUPPLY_STATUS_CHARGING;
+			} else if ((di->usb_in || di->ta_in) && gpio_get_value(di->pdata->chg) == 1) {
+				val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
+			} else {
+				val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
+			}
+		}
+		di->battery_status = val->intval;
+		return 0;
+	default:
+		break;
+	}
+	if (!last || time_after(jiffies, last + HZ / 2)) {
+		last = jiffies;
+		max8903_charger_update_status(di);
+		max8903_battery_update_status(di);
+	}
+	switch (psp) {
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		val->intval = di->voltage_uV;
+		break;
+#if 0
+	case POWER_SUPPLY_PROP_CURRENT_NOW:
+		val->intval = di->current_uA;
+		break;
+#endif
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		val->intval = 0;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
+		val->intval = HIGH_VOLT_THRESHOLD;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
+		val->intval = LOW_VOLT_THRESHOLD;
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		val->intval = 1;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY:
+		val->intval = di->percent < 0 ? 0 :
+				(di->percent > 100 ? 100 : di->percent);
+		break;
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = 0;
+		if (di->usb_in || di->ta_in)
+			val->intval = 1;
+		di->charger_online = val->intval;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+static int max8903_get_property(struct power_supply *psy,
+		enum power_supply_property psp,
+		union power_supply_propval *val)
+{
+	struct max8903_data *data = container_of(psy,
+			struct max8903_data, psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
+		if (data->pdata->chg) {
+			if (gpio_get_value(data->pdata->chg) == 0) {
+				val->intval = POWER_SUPPLY_STATUS_CHARGING;
+				}
+			else if ((data->usb_in || data->ta_in) &&
+				gpio_get_value(data->pdata->chg) == 1) {
+				val->intval = POWER_SUPPLY_STATUS_NOT_CHARGING;
+			} else {
+				val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
+			}
+		}
+		data->battery_status = val->intval;
+		break;
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = 0;
+		if (data->usb_in || data->ta_in)
+			val->intval = 1;
+		data->charger_online = val->intval;
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		if (data->fault)
+			val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static irqreturn_t max8903_dcin(int irq, void *_data)
+{
+	struct max8903_data *data = _data;
+	struct max8903_pdata *pdata = data->pdata;
+	bool ta_in;
+	enum power_supply_type old_type;
+
+	ta_in = gpio_get_value(pdata->dok) ? false : true;
+
+	if (ta_in == data->ta_in)
+		return IRQ_HANDLED;
+
+	data->ta_in = ta_in;
+#if 0
+	/* Set Current-Limit-Mode 1:DC 0:USB */
+	if (pdata->dcm)
+		gpio_set_value(pdata->dcm, ta_in ? 1 : 0);
+	/* Charger Enable / Disable (cen is negated) */
+	if (pdata->cen)
+		gpio_set_value(pdata->cen, ta_in ? 0 :
+				(data->usb_in ? 0 : 1));
+#endif
+	pr_info("TA(DC-IN) Charger %s.\n", ta_in ?
+			"Connected" : "Disconnected");
+
+	old_type = data->psy.type;
+
+	if (data->ta_in)
+		data->psy.type = POWER_SUPPLY_TYPE_MAINS;
+	else if (data->usb_in)
+		data->psy.type = POWER_SUPPLY_TYPE_USB;
+	else
+		data->psy.type = POWER_SUPPLY_TYPE_BATTERY;
+
+	if (old_type != data->psy.type) {
+		power_supply_changed(&data->psy);
+		power_supply_changed(&data->bat);
+		}
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t max8903_usbin(int irq, void *_data)
+{
+	struct max8903_data *data = _data;
+	struct max8903_pdata *pdata = data->pdata;
+	bool usb_in;
+	enum power_supply_type old_type;
+
+	usb_in = gpio_get_value(pdata->uok) ? false : true;
+
+	if (usb_in == data->usb_in)
+		return IRQ_HANDLED;
+
+	data->usb_in = usb_in;
+
+#if 0
+	/* Do not touch Current-Limit-Mode */
+
+	/* Charger Enable / Disable (cen is negated) */
+	if (pdata->cen)
+		gpio_set_value(pdata->cen, usb_in ? 0 :
+				(data->ta_in ? 0 : 1));
+#endif
+	pr_info("USB Charger %s.\n", usb_in ?
+			"Connected" : "Disconnected");
+
+	old_type = data->psy.type;
+
+	if (data->ta_in)
+		data->psy.type = POWER_SUPPLY_TYPE_MAINS;
+	else if (data->usb_in)
+		data->psy.type = POWER_SUPPLY_TYPE_USB;
+	else
+		data->psy.type = POWER_SUPPLY_TYPE_BATTERY;
+
+	if (old_type != data->psy.type) {
+		power_supply_changed(&data->psy);
+		power_supply_changed(&data->bat);
+		}
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t max8903_fault(int irq, void *_data)
+{
+	struct max8903_data *data = _data;
+	struct max8903_pdata *pdata = data->pdata;
+	bool fault;
+
+	fault = gpio_get_value(pdata->flt) ? false : true;
+
+	if (fault == data->fault)
+		return IRQ_HANDLED;
+
+	data->fault = fault;
+
+	if (fault)
+		dev_err(data->dev, "Charger suffers a fault and stops.\n");
+	else
+		dev_err(data->dev, "Charger recovered from a fault.\n");
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t max8903_chg(int irq, void *_data)
+{
+	struct max8903_data *data = _data;
+	struct max8903_pdata *pdata = data->pdata;
+	int chg_state;
+
+	chg_state = gpio_get_value(pdata->chg) ? false : true;
+
+	if (chg_state == data->chg_state)
+		return IRQ_HANDLED;
+
+	data->chg_state = chg_state;
+#if 0
+	if (chg_state)
+		pr_info("Charger stop.\n ");
+	else
+		pr_info("Charger start.\n ");
+#endif
+	return IRQ_HANDLED;
+}
+
+static void max8903_battery_work(struct work_struct *work)
+{
+	struct max8903_data *data;
+	data = container_of(work, struct max8903_data, work.work);
+	data->interval = HZ * BATTERY_UPDATE_INTERVAL;
+
+	max8903_charger_update_status(data);
+	max8903_battery_update_status(data);
+
+	pr_debug("battery voltage: %4d mV\n" , data->voltage_uV);
+	pr_debug("charger online status: %d\n" , data->charger_online);
+	pr_debug("battery status : %d\n" , data->battery_status);
+	pr_debug("battery capacity percent: %3d\n" , data->percent);
+	pr_debug("data->usb_in: %x , data->ta_in: %x \n" , data->usb_in, data->ta_in);
+	/* reschedule for the next time */
+	schedule_delayed_work(&data->work, data->interval);
+}
+static __devinit int max8903_probe(struct platform_device *pdev)
+{
+	struct max8903_data *data;
+	struct device *dev = &pdev->dev;
+	struct max8903_pdata *pdata = pdev->dev.platform_data;
+	int ret = 0;
+	int gpio = 0;
+	int ta_in = 0;
+	int usb_in = 0;
+	int retval;
+
+	data = kzalloc(sizeof(struct max8903_data), GFP_KERNEL);
+	if (data == NULL) {
+		dev_err(dev, "Cannot allocate memory.\n");
+		return -ENOMEM;
+	}
+	data->pdata = pdata;
+	data->dev = dev;
+
+	platform_set_drvdata(pdev, data);
+	capacity_changed_flag = false;
+	data->usb_in = 0;
+	data->ta_in = 0;
+
+	if (pdata->dc_valid == false && pdata->usb_valid == false) {
+		dev_err(dev, "No valid power sources.\n");
+		printk(KERN_INFO "No valid power sources.\n");
+		ret = -EINVAL;
+		goto err;
+	}
+	if (pdata->dc_valid) {
+#if 0
+		if (pdata->dok && gpio_is_valid(pdata->dok) &&
+				pdata->dcm && gpio_is_valid(pdata->dcm)) {
+#endif
+		if (pdata->dok && gpio_is_valid(pdata->dok)) {
+			gpio = pdata->dok; /* PULL_UPed Interrupt */
+			ta_in = gpio_get_value(gpio) ? 0 : 1;
+#if 0
+			gpio = pdata->dcm; /* Output */
+			gpio_set_value(gpio, ta_in);
+#endif
+		} else if (pdata->dok && gpio_is_valid(pdata->dok) && pdata->dcm_always_high) {
+			ta_in = pdata->dok; /* PULL_UPed Interrupt */
+			ta_in = gpio_get_value(gpio) ? 0 : 1;
+		} else {
+			dev_err(dev, "When DC is wired, DOK and DCM should"
+					" be wired as well."
+					" or set dcm always high\n");
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+	if (pdata->usb_valid) {
+		if (pdata->uok && gpio_is_valid(pdata->uok)) {
+			gpio = pdata->uok;
+			usb_in = gpio_get_value(gpio) ? 0 : 1;
+		} else {
+			dev_err(dev, "When USB is wired, UOK should be wired."
+					"as well.\n");
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+	if (pdata->chg) {
+		if (!gpio_is_valid(pdata->chg)) {
+			dev_err(dev, "Invalid pin: chg.\n");
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+
+	if (pdata->flt) {
+		if (!gpio_is_valid(pdata->flt)) {
+			dev_err(dev, "Invalid pin: flt.\n");
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+
+	if (pdata->usus) {
+		if (!gpio_is_valid(pdata->usus)) {
+			dev_err(dev, "Invalid pin: usus.\n");
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+	mutex_init(&data->work_lock);
+	data->fault = false;
+	data->ta_in = ta_in;
+	data->usb_in = usb_in;
+	data->psy.name = "max8903-ac";
+	data->psy.type = (ta_in) ? POWER_SUPPLY_TYPE_MAINS :
+			((usb_in) ? POWER_SUPPLY_TYPE_USB :
+			 POWER_SUPPLY_TYPE_BATTERY);
+	data->psy.get_property = max8903_get_property;
+	data->psy.properties = max8903_charger_props;
+	data->psy.num_properties = ARRAY_SIZE(max8903_charger_props);
+	ret = power_supply_register(dev, &data->psy);
+	if (ret) {
+		dev_err(dev, "failed: power supply register.\n");
+		goto err_psy;
+	}
+	data->bat.name = "max8903-charger";
+	data->bat.type = POWER_SUPPLY_TYPE_BATTERY;
+	data->bat.properties = max8903_battery_props;
+	data->bat.num_properties = ARRAY_SIZE(max8903_battery_props);
+	data->bat.get_property = max8903_battery_get_property;
+	data->bat.use_for_apm = 1;
+	retval = power_supply_register(&pdev->dev, &data->bat);
+	if (retval) {
+		dev_err(data->dev, "failed to register battery\n");
+		goto battery_failed;
+	}
+	INIT_DELAYED_WORK(&data->work, max8903_battery_work);
+	schedule_delayed_work(&data->work, data->interval);
+	if (pdata->dc_valid) {
+		ret = request_threaded_irq(gpio_to_irq(pdata->dok),
+				NULL, max8903_dcin,
+				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				"MAX8903 DC IN", data);
+		if (ret) {
+			dev_err(dev, "Cannot request irq %d for DC (%d)\n",
+					gpio_to_irq(pdata->dok), ret);
+			goto err_usb_irq;
+		}
+	}
+
+	if (pdata->usb_valid) {
+		ret = request_threaded_irq(gpio_to_irq(pdata->uok),
+				NULL, max8903_usbin,
+				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				"MAX8903 USB IN", data);
+		if (ret) {
+			dev_err(dev, "Cannot request irq %d for USB (%d)\n",
+					gpio_to_irq(pdata->uok), ret);
+			goto err_dc_irq;
+		}
+	}
+
+	if (pdata->flt) {
+		ret = request_threaded_irq(gpio_to_irq(pdata->flt),
+				NULL, max8903_fault,
+				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				"MAX8903 Fault", data);
+		if (ret) {
+			dev_err(dev, "Cannot request irq %d for Fault (%d)\n",
+					gpio_to_irq(pdata->flt), ret);
+			goto err_flt_irq;
+		}
+	}
+
+	if (pdata->chg) {
+		ret = request_threaded_irq(gpio_to_irq(pdata->chg),
+				NULL, max8903_chg,
+				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				"MAX8903 Fault", data);
+		if (ret) {
+			dev_err(dev, "Cannot request irq %d for Fault (%d)\n",
+					gpio_to_irq(pdata->flt), ret);
+			goto err_chg_irq;
+		}
+	}
+	return 0;
+err_psy:
+	power_supply_unregister(&data->psy);
+battery_failed:
+	power_supply_unregister(&data->bat);
+err_usb_irq:
+	if (pdata->usb_valid)
+		free_irq(gpio_to_irq(pdata->uok), data);
+	cancel_delayed_work(&data->work);
+err_dc_irq:
+	if (pdata->dc_valid)
+		free_irq(gpio_to_irq(pdata->dok), data);
+	cancel_delayed_work(&data->work);
+err_flt_irq:
+	if (pdata->usb_valid)
+		free_irq(gpio_to_irq(pdata->uok), data);
+	cancel_delayed_work(&data->work);
+err_chg_irq:
+	if (pdata->dc_valid)
+		free_irq(gpio_to_irq(pdata->dok), data);
+	cancel_delayed_work(&data->work);
+err:
+	kfree(data);
+	return ret;
+}
+
+static __devexit int max8903_remove(struct platform_device *pdev)
+{
+	struct max8903_data *data = platform_get_drvdata(pdev);
+	if (data) {
+		struct max8903_pdata *pdata = data->pdata;
+		if (pdata->flt)
+			free_irq(gpio_to_irq(pdata->flt), data);
+		if (pdata->usb_valid)
+			free_irq(gpio_to_irq(pdata->uok), data);
+		if (pdata->dc_valid)
+			free_irq(gpio_to_irq(pdata->dok), data);
+		if (pdata->dc_valid)
+			free_irq(gpio_to_irq(pdata->chg), data);
+		cancel_delayed_work_sync(&data->work);
+		power_supply_unregister(&data->psy);
+		power_supply_unregister(&data->bat);
+		kfree(data);
+	}
+	return 0;
+}
+
+static int max8903_suspend(struct platform_device *pdev,
+				  pm_message_t state)
+{
+	struct max8903_data *data = platform_get_drvdata(pdev);
+
+	cancel_delayed_work(&data->work);
+	return 0;
+}
+
+static int max8903_resume(struct platform_device *pdev)
+{
+	struct max8903_data *data = platform_get_drvdata(pdev);
+
+	schedule_delayed_work(&data->work, BATTERY_UPDATE_INTERVAL);
+	return 0;
+
+}
+
+static struct platform_driver max8903_driver = {
+	.probe	= max8903_probe,
+	.remove	= __devexit_p(max8903_remove),
+	.suspend = max8903_suspend,
+	.resume = max8903_resume,
+	.driver = {
+		.name	= "max8903-charger",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init max8903_init(void)
+{
+	return platform_driver_register(&max8903_driver);
+}
+module_init(max8903_init);
+
+static void __exit max8903_exit(void)
+{
+	platform_driver_unregister(&max8903_driver);
+}
+module_exit(max8903_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MAX8903 Battery Driver");
+MODULE_ALIAS("max8903_battery");
diff --git a/drivers/power/max8903_charger.c b/drivers/power/max8903_charger.c
index 492df4310228..fee8191767f0 100644
--- a/drivers/power/max8903_charger.c
+++ b/drivers/power/max8903_charger.c
@@ -246,13 +246,13 @@ static __devinit int max8903_probe(struct platform_device *pdev)
 			gpio = pdata->dok; /* PULL_UPed Interrupt */
 
 			error = gpio_request(gpio, "chg_dc");
-			error = gpio_direction_input(gpio);
 			if (error < 0) {
 				dev_err(dev, "failed to configure"
 					" request/direction for GPIO %d, error %d\n",
 					gpio, error);
 				goto err;
 			}
+			gpio_direction_input(gpio);
 
 			ta_in = gpio_get_value(gpio) ? 0 : 1;
 
@@ -283,15 +283,15 @@ static __devinit int max8903_probe(struct platform_device *pdev)
 		if (pdata->uok && gpio_is_valid(pdata->uok)) {
 			gpio = pdata->uok;
 			error = gpio_request(gpio, "chg_usb");
-			error = gpio_direction_input(gpio);
 			if (error < 0) {
 				dev_err(dev, "failed to configure"
 					" request/direction for GPIO %d, error %d\n",
 					gpio, error);
 				goto err;
 			}
-			usb_in = gpio_get_value(gpio) ? 0 : 1;
 
+			gpio_direction_input(gpio);
+			usb_in = gpio_get_value(gpio) ? 0 : 1;
 			if (usb_in)
 				data->usb_in = true;
 			else
@@ -321,13 +321,13 @@ static __devinit int max8903_probe(struct platform_device *pdev)
 			goto err;
 		}
 		error = gpio_request(pdata->chg, "chg_status");
-		error = gpio_direction_input(pdata->chg);
 		if (error < 0) {
 			dev_err(dev, "failed to configure"
 				" request/direction for GPIO %d, error %d\n",
 				pdata->chg, error);
 			goto err;
 		}
+		error = gpio_direction_input(pdata->chg);
 	}
 
 	if (pdata->flt) {
@@ -337,13 +337,13 @@ static __devinit int max8903_probe(struct platform_device *pdev)
 			goto err;
 		}
 		error = gpio_request(pdata->flt, "chg_fault");
-		error = gpio_direction_input(pdata->flt);
 		if (error < 0) {
 			dev_err(dev, "failed to configure"
 				" request/direction for GPIO %d, error %d\n",
 				pdata->flt, error);
 			goto err;
 		}
+		error = gpio_direction_input(pdata->flt);
 	}
 
 	if (pdata->usus) {
diff --git a/drivers/power/sabresd_battery.c b/drivers/power/sabresd_battery.c
new file mode 100755
index 000000000000..75d07b178083
--- /dev/null
+++ b/drivers/power/sabresd_battery.c
@@ -0,0 +1,701 @@
+/*
+ * sabresd_battery.c - Maxim 8903 USB/Adapter Charger Driver
+ *
+ * Copyright (C) 2011 Samsung Electronics
+ * Copyright (C) 2011-2012 Freescale Semiconductor, Inc.
+ * Based on max8903_charger.c
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/power_supply.h>
+#include <linux/platform_device.h>
+#include <linux/power/sabresd_battery.h>
+#include <linux/sort.h>
+
+
+#define	BATTERY_UPDATE_INTERVAL	5 /*seconds*/
+#define LOW_VOLT_THRESHOLD	2800000
+#define HIGH_VOLT_THRESHOLD	4200000
+#define ADC_SAMPLE_COUNT	6
+
+struct max8903_data {
+	struct max8903_pdata *pdata;
+	struct device *dev;
+	struct power_supply psy;
+	bool fault;
+	bool usb_in;
+	bool ta_in;
+	bool chg_state;
+	struct delayed_work work;
+	unsigned int interval;
+	unsigned short thermal_raw;
+	int voltage_uV;
+	int current_uA;
+	int battery_status;
+	int charger_online;
+	int charger_voltage_uV;
+	int real_capacity;
+	int percent;
+	int old_percent;
+	struct power_supply bat;
+	struct mutex work_lock;
+};
+
+typedef struct {
+	u32 voltage;
+	u32 percent;
+} battery_capacity , *pbattery_capacity;
+
+static bool capacity_changed_flag;
+
+static battery_capacity chargingTable[] = {
+    {4146, 100},
+    {4133,  99},
+    {4123,  98},
+    {4115,  97},
+    {4090,  96},
+    {4075,  95},
+    {4060,  94},
+    {4045,  93},
+    {4030,  92},
+    {4015,  91},
+    {4000,  90},
+    {3900,  85},
+    {3790,  80},
+    {3760,  75},
+    {3730,  70},
+    {3700,  65},
+    {3680,  60},
+    {3660,  55},
+    {3640,  50},
+    {3600,  45},
+    {3550,  40},
+    {3510,  35},
+    {3450,  30},
+    {3310,  25},
+    {3240,  20},
+    {3180,  15},
+    {3030,  10},
+    {2820,  5},
+    {2800,  0},
+    {0,  0}
+};
+static battery_capacity dischargingTable[] = {
+    {4110, 100},
+    {4020,  99},
+    {3950,  98},
+    {3920,  97},
+    {3890,  96},
+    {3860,  96},
+    {3830,  95},
+    {3800,  94},
+    {3760,  93},
+    {3730,  92},
+    {3700,  91},
+    {3670,  90},
+    {3630,  85},
+    {3600,  80},
+    {3570,  75},
+    {3545,  70},
+    {3515,  65},
+    {3480,  60},
+    {3445,  55},
+    {3410,  50},
+    {3375,  45},
+    {3340,  40},
+    {3300,  35},
+    {3250,  30},
+    {3200,  25},
+    {3150,  20},
+    {3090,  15},
+    {3020,  10},
+    {2820,  5},
+    {2800,  0},
+    {0,  0}
+};
+
+u32 calibrate_battery_capability_percent(struct max8903_data *data)
+{
+    u8 i;
+    pbattery_capacity pTable;
+    u32 tableSize;
+    if (data->battery_status  == POWER_SUPPLY_STATUS_DISCHARGING) {
+		pTable = dischargingTable;
+		tableSize = sizeof(dischargingTable)/sizeof(dischargingTable[0]);
+    } else {
+		pTable = chargingTable;
+		tableSize = sizeof(chargingTable)/sizeof(chargingTable[0]);
+    }
+    for (i = 0; i < tableSize; i++) {
+		if (data->voltage_uV >= pTable[i].voltage)
+			return	pTable[i].percent;
+    }
+
+    return 0;
+}
+
+static enum power_supply_property max8903_charger_props[] = {
+	POWER_SUPPLY_PROP_ONLINE,
+};
+
+static enum power_supply_property max8903_battery_props[] = {
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN,
+	POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN,
+	POWER_SUPPLY_PROP_HEALTH,
+};
+
+extern u32 max11801_read_adc(void);
+
+static void max8903_charger_update_status(struct max8903_data *data)
+{
+	if (data->usb_in || data->ta_in)
+		data->charger_online = 1;
+	else
+		data->charger_online = 0;
+	if (data->charger_online == 0) {
+			data->battery_status = POWER_SUPPLY_STATUS_DISCHARGING;
+		} else {
+			if (data->pdata->chg) {
+					if (gpio_get_value(data->pdata->chg) == 0) {
+						data->battery_status = POWER_SUPPLY_STATUS_CHARGING;
+					} else if ((data->usb_in || data->ta_in) &&
+						gpio_get_value(data->pdata->chg) == 1) {
+						if (data->percent == 100)
+							data->battery_status = POWER_SUPPLY_STATUS_FULL;
+						else
+							data->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+						}
+			}
+		}
+	pr_debug("chg: %d \n", gpio_get_value(data->pdata->chg));
+}
+static int cmp_func(const void *_a, const void *_b)
+{
+	const int *a = _a, *b = _b;
+
+	if (*a > *b)
+		return 1;
+	if (*a < *b)
+		return -1;
+	return 0;
+}
+u32 calibration_voltage(struct max8903_data *data)
+{
+	int volt[ADC_SAMPLE_COUNT];
+	u32 voltage_data;
+	int i;
+	for (i = 0; i < ADC_SAMPLE_COUNT; i++) {
+		if (data->charger_online == 0) {
+			/* ADC offset when battery is discharger*/
+			volt[i] = max11801_read_adc()-1494;
+			} else {
+			volt[i] = max11801_read_adc()-1545;
+			}
+	}
+	sort(volt, i, 4, cmp_func, NULL);
+	for (i = 0; i < ADC_SAMPLE_COUNT; i++)
+		pr_debug("volt_sorted[%2d]: %d\n", i, volt[i]);
+	/* get the average of second max/min of remained. */
+	voltage_data = (volt[2] + volt[ADC_SAMPLE_COUNT - 3]) / 2;
+	return voltage_data;
+}
+
+static void max8903_battery_update_status(struct max8903_data *data)
+{
+	static int counter;
+	mutex_lock(&data->work_lock);
+	data->voltage_uV = calibration_voltage(data);
+	data->percent = calibrate_battery_capability_percent(data);
+	if (data->percent != data->old_percent) {
+		data->old_percent = data->percent;
+		capacity_changed_flag = true;
+	}
+	if ((capacity_changed_flag == true) && (data->charger_online)) {
+		counter++;
+		if (counter > 2) {
+			counter = 0;
+			capacity_changed_flag = false;
+			power_supply_changed(&data->bat);
+		}
+	}
+	mutex_unlock(&data->work_lock);
+}
+
+static int max8903_battery_get_property(struct power_supply *bat,
+				       enum power_supply_property psp,
+				       union power_supply_propval *val)
+{
+	struct max8903_data *di = container_of(bat,
+			struct max8903_data, bat);
+	static unsigned long last;
+	switch (psp) {
+	case POWER_SUPPLY_PROP_STATUS:
+		val->intval = POWER_SUPPLY_STATUS_UNKNOWN;
+		if (di->pdata->chg) {
+			if ((di->usb_in || di->ta_in) && gpio_get_value(di->pdata->chg) == 0) {
+				val->intval = POWER_SUPPLY_STATUS_CHARGING;
+			} else if ((di->usb_in || di->ta_in) && gpio_get_value(di->pdata->chg) == 1) {
+				if (di->percent == 100)
+					di->battery_status = POWER_SUPPLY_STATUS_FULL;
+				else
+					di->battery_status = POWER_SUPPLY_STATUS_NOT_CHARGING;
+			}
+			  else {
+				val->intval = POWER_SUPPLY_STATUS_DISCHARGING;
+			}
+		}
+		di->battery_status = val->intval;
+		return 0;
+	default:
+		break;
+	}
+	if (!last || time_after(jiffies, last + HZ / 2)) {
+		last = jiffies;
+		max8903_charger_update_status(di);
+		max8903_battery_update_status(di);
+	}
+	switch (psp) {
+	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+		val->intval = di->voltage_uV;
+		break;
+	case POWER_SUPPLY_PROP_CHARGE_NOW:
+		val->intval = 0;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
+		val->intval = HIGH_VOLT_THRESHOLD;
+		break;
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
+		val->intval = LOW_VOLT_THRESHOLD;
+		break;
+	case POWER_SUPPLY_PROP_PRESENT:
+		val->intval = 1;
+		break;
+	case POWER_SUPPLY_PROP_CAPACITY:
+		val->intval = di->percent < 0 ? 0 :
+				(di->percent > 100 ? 100 : di->percent);
+		break;
+	case POWER_SUPPLY_PROP_HEALTH:
+		val->intval = POWER_SUPPLY_HEALTH_GOOD;
+		if (di->fault)
+			val->intval = POWER_SUPPLY_HEALTH_UNSPEC_FAILURE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+static int max8903_get_property(struct power_supply *psy,
+		enum power_supply_property psp,
+		union power_supply_propval *val)
+{
+	struct max8903_data *data = container_of(psy,
+			struct max8903_data, psy);
+
+	switch (psp) {
+	case POWER_SUPPLY_PROP_ONLINE:
+		val->intval = 0;
+		if (data->usb_in || data->ta_in)
+			val->intval = 1;
+		data->charger_online = val->intval;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static irqreturn_t max8903_dcin(int irq, void *_data)
+{
+	struct max8903_data *data = _data;
+	struct max8903_pdata *pdata = data->pdata;
+	bool ta_in;
+
+	ta_in = gpio_get_value(pdata->dok) ? false : true;
+
+	if (ta_in == data->ta_in)
+		return IRQ_HANDLED;
+
+	data->ta_in = ta_in;
+	pr_info("TA(DC-IN) Charger %s.\n", ta_in ?
+			"Connected" : "Disconnected");
+	power_supply_changed(&data->psy);
+	power_supply_changed(&data->bat);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t max8903_usbin(int irq, void *_data)
+{
+	struct max8903_data *data = _data;
+	struct max8903_pdata *pdata = data->pdata;
+	bool usb_in;
+
+	usb_in = gpio_get_value(pdata->uok) ? false : true;
+
+	if (usb_in == data->usb_in)
+		return IRQ_HANDLED;
+
+	data->usb_in = usb_in;
+
+	pr_info("USB Charger %s.\n", usb_in ?
+			"Connected" : "Disconnected");
+	power_supply_changed(&data->psy);
+	power_supply_changed(&data->bat);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t max8903_fault(int irq, void *_data)
+{
+	struct max8903_data *data = _data;
+	struct max8903_pdata *pdata = data->pdata;
+	bool fault;
+
+	fault = gpio_get_value(pdata->flt) ? false : true;
+
+	if (fault == data->fault)
+		return IRQ_HANDLED;
+
+	data->fault = fault;
+
+	if (fault)
+		dev_err(data->dev, "Charger suffers a fault and stops.\n");
+	else
+		dev_err(data->dev, "Charger recovered from a fault.\n");
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t max8903_chg(int irq, void *_data)
+{
+	struct max8903_data *data = _data;
+	struct max8903_pdata *pdata = data->pdata;
+	int chg_state;
+
+	chg_state = gpio_get_value(pdata->chg) ? false : true;
+
+	if (chg_state == data->chg_state)
+		return IRQ_HANDLED;
+
+	data->chg_state = chg_state;
+	return IRQ_HANDLED;
+}
+
+static void max8903_battery_work(struct work_struct *work)
+{
+	struct max8903_data *data;
+	data = container_of(work, struct max8903_data, work.work);
+	data->interval = HZ * BATTERY_UPDATE_INTERVAL;
+
+	max8903_charger_update_status(data);
+	max8903_battery_update_status(data);
+
+	pr_debug("battery voltage: %4d mV\n" , data->voltage_uV);
+	pr_debug("charger online status: %d\n" , data->charger_online);
+	pr_debug("battery status : %d\n" , data->battery_status);
+	pr_debug("battery capacity percent: %3d\n" , data->percent);
+	pr_debug("data->usb_in: %x , data->ta_in: %x \n" , data->usb_in, data->ta_in);
+	/* reschedule for the next time */
+	schedule_delayed_work(&data->work, data->interval);
+}
+static __devinit int max8903_probe(struct platform_device *pdev)
+{
+	struct max8903_data *data;
+	struct device *dev = &pdev->dev;
+	struct max8903_pdata *pdata = pdev->dev.platform_data;
+	int ret = 0;
+	int gpio = 0;
+	int ta_in = 0;
+	int usb_in = 0;
+	int retval;
+
+	data = kzalloc(sizeof(struct max8903_data), GFP_KERNEL);
+	if (data == NULL) {
+		dev_err(dev, "Cannot allocate memory.\n");
+		return -ENOMEM;
+	}
+	data->pdata = pdata;
+	data->dev = dev;
+
+	platform_set_drvdata(pdev, data);
+	capacity_changed_flag = false;
+	data->usb_in = 0;
+	data->ta_in = 0;
+
+	if (pdata->dc_valid == false && pdata->usb_valid == false) {
+		dev_err(dev, "No valid power sources.\n");
+		printk(KERN_INFO "No valid power sources.\n");
+		ret = -EINVAL;
+		goto err;
+	}
+	if (pdata->dc_valid) {
+		if (pdata->dok && gpio_is_valid(pdata->dok)) {
+			gpio = pdata->dok; /* PULL_UPed Interrupt */
+			/* set DOK gpio input */
+			ret = gpio_request(gpio, "max8903-DOK");
+			if (ret) {
+				printk(KERN_ERR"request max8903-DOK error!!\n");
+				goto err;
+			} else {
+				gpio_direction_input(gpio);
+			}
+			ta_in = gpio_get_value(gpio) ? 0 : 1;
+		} else if (pdata->dok && gpio_is_valid(pdata->dok) && pdata->dcm_always_high) {
+			ta_in = pdata->dok; /* PULL_UPed Interrupt */
+			ta_in = gpio_get_value(gpio) ? 0 : 1;
+		} else {
+			dev_err(dev, "When DC is wired, DOK and DCM should"
+					" be wired as well."
+					" or set dcm always high\n");
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+	if (pdata->usb_valid) {
+		if (pdata->uok && gpio_is_valid(pdata->uok)) {
+			gpio = pdata->uok;
+			/* set UOK gpio input */
+			ret = gpio_request(gpio, "max8903-UOK");
+			if (ret) {
+				printk(KERN_ERR"request max8903-UOK error!!\n");
+				goto err;
+			} else {
+				gpio_direction_input(gpio);
+			}
+			usb_in = gpio_get_value(gpio) ? 0 : 1;
+		} else {
+			dev_err(dev, "When USB is wired, UOK should be wired."
+					"as well.\n");
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+	if (pdata->chg) {
+		if (!gpio_is_valid(pdata->chg)) {
+			dev_err(dev, "Invalid pin: chg.\n");
+			ret = -EINVAL;
+			goto err;
+		}
+		/* set CHG gpio input */
+		ret = gpio_request(pdata->chg, "max8903-CHG");
+		if (ret) {
+			printk(KERN_ERR"request max8903-CHG error!!\n");
+			goto err;
+		} else {
+			gpio_direction_input(pdata->chg);
+		}
+	}
+	if (pdata->flt) {
+		if (!gpio_is_valid(pdata->flt)) {
+			dev_err(dev, "Invalid pin: flt.\n");
+			ret = -EINVAL;
+			goto err;
+		}
+		/* set FLT gpio input */
+		ret = gpio_request(pdata->flt, "max8903-FLT");
+		if (ret) {
+			printk(KERN_ERR"request max8903-FLT error!!\n");
+			goto err;
+		} else {
+			gpio_direction_input(pdata->flt);
+		}
+	}
+	if (pdata->usus) {
+		if (!gpio_is_valid(pdata->usus)) {
+			dev_err(dev, "Invalid pin: usus.\n");
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+	mutex_init(&data->work_lock);
+	data->fault = false;
+	data->ta_in = ta_in;
+	data->usb_in = usb_in;
+	data->psy.name = "max8903-ac";
+	data->psy.type = POWER_SUPPLY_TYPE_MAINS;
+	data->psy.get_property = max8903_get_property;
+	data->psy.properties = max8903_charger_props;
+	data->psy.num_properties = ARRAY_SIZE(max8903_charger_props);
+	ret = power_supply_register(dev, &data->psy);
+	if (ret) {
+		dev_err(dev, "failed: power supply register.\n");
+		goto err_psy;
+	}
+	data->bat.name = "max8903-charger";
+	data->bat.type = POWER_SUPPLY_TYPE_BATTERY;
+	data->bat.properties = max8903_battery_props;
+	data->bat.num_properties = ARRAY_SIZE(max8903_battery_props);
+	data->bat.get_property = max8903_battery_get_property;
+	data->bat.use_for_apm = 1;
+	retval = power_supply_register(&pdev->dev, &data->bat);
+	if (retval) {
+		dev_err(data->dev, "failed to register battery\n");
+		goto battery_failed;
+	}
+	INIT_DELAYED_WORK(&data->work, max8903_battery_work);
+	schedule_delayed_work(&data->work, data->interval);
+	if (pdata->dc_valid) {
+		ret = request_threaded_irq(gpio_to_irq(pdata->dok),
+				NULL, max8903_dcin,
+				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				"MAX8903 DC IN", data);
+		if (ret) {
+			dev_err(dev, "Cannot request irq %d for DC (%d)\n",
+					gpio_to_irq(pdata->dok), ret);
+			goto err_usb_irq;
+		}
+	}
+
+	if (pdata->usb_valid) {
+		ret = request_threaded_irq(gpio_to_irq(pdata->uok),
+				NULL, max8903_usbin,
+				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				"MAX8903 USB IN", data);
+		if (ret) {
+			dev_err(dev, "Cannot request irq %d for USB (%d)\n",
+					gpio_to_irq(pdata->uok), ret);
+			goto err_dc_irq;
+		}
+	}
+
+	if (pdata->flt) {
+		ret = request_threaded_irq(gpio_to_irq(pdata->flt),
+				NULL, max8903_fault,
+				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				"MAX8903 Fault", data);
+		if (ret) {
+			dev_err(dev, "Cannot request irq %d for Fault (%d)\n",
+					gpio_to_irq(pdata->flt), ret);
+			goto err_flt_irq;
+		}
+	}
+
+	if (pdata->chg) {
+		ret = request_threaded_irq(gpio_to_irq(pdata->chg),
+				NULL, max8903_chg,
+				IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+				"MAX8903 Fault", data);
+		if (ret) {
+			dev_err(dev, "Cannot request irq %d for Fault (%d)\n",
+					gpio_to_irq(pdata->flt), ret);
+			goto err_chg_irq;
+		}
+	}
+	return 0;
+err_psy:
+	power_supply_unregister(&data->psy);
+battery_failed:
+	power_supply_unregister(&data->bat);
+err_usb_irq:
+	if (pdata->usb_valid)
+		free_irq(gpio_to_irq(pdata->uok), data);
+	cancel_delayed_work(&data->work);
+err_dc_irq:
+	if (pdata->dc_valid)
+		free_irq(gpio_to_irq(pdata->dok), data);
+	cancel_delayed_work(&data->work);
+err_flt_irq:
+	if (pdata->usb_valid)
+		free_irq(gpio_to_irq(pdata->uok), data);
+	cancel_delayed_work(&data->work);
+err_chg_irq:
+	if (pdata->dc_valid)
+		free_irq(gpio_to_irq(pdata->dok), data);
+	cancel_delayed_work(&data->work);
+err:
+	if (pdata->uok)
+		gpio_free(pdata->uok);
+	if (pdata->dok)
+		gpio_free(pdata->dok);
+	if (pdata->flt)
+		gpio_free(pdata->flt);
+	if (pdata->chg)
+		gpio_free(pdata->chg);
+	kfree(data);
+	return ret;
+}
+
+static __devexit int max8903_remove(struct platform_device *pdev)
+{
+	struct max8903_data *data = platform_get_drvdata(pdev);
+	if (data) {
+		struct max8903_pdata *pdata = data->pdata;
+		if (pdata->flt)
+			free_irq(gpio_to_irq(pdata->flt), data);
+		if (pdata->usb_valid)
+			free_irq(gpio_to_irq(pdata->uok), data);
+		if (pdata->dc_valid)
+			free_irq(gpio_to_irq(pdata->dok), data);
+		if (pdata->dc_valid)
+			free_irq(gpio_to_irq(pdata->chg), data);
+		cancel_delayed_work_sync(&data->work);
+		power_supply_unregister(&data->psy);
+		power_supply_unregister(&data->bat);
+		kfree(data);
+	}
+	return 0;
+}
+
+static int max8903_suspend(struct platform_device *pdev,
+				  pm_message_t state)
+{
+	struct max8903_data *data = platform_get_drvdata(pdev);
+
+	cancel_delayed_work(&data->work);
+	return 0;
+}
+
+static int max8903_resume(struct platform_device *pdev)
+{
+	struct max8903_data *data = platform_get_drvdata(pdev);
+
+	schedule_delayed_work(&data->work, BATTERY_UPDATE_INTERVAL);
+	return 0;
+
+}
+
+static struct platform_driver max8903_driver = {
+	.probe	= max8903_probe,
+	.remove	= __devexit_p(max8903_remove),
+	.suspend = max8903_suspend,
+	.resume = max8903_resume,
+	.driver = {
+		.name	= "max8903-charger",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init max8903_init(void)
+{
+	return platform_driver_register(&max8903_driver);
+}
+module_init(max8903_init);
+
+static void __exit max8903_exit(void)
+{
+	platform_driver_unregister(&max8903_driver);
+}
+module_exit(max8903_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("Sabresd Battery Driver");
+MODULE_ALIAS("sabresd_battery");
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 0a6deb268b38..a79163f0ae40 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -276,7 +276,7 @@ static ssize_t regulator_uV_show(struct device *dev,
 	return ret;
 }
 static ssize_t regulator_uV_store(struct device *dev,
-				struct device_attribute *attr, char *buf,
+				struct device_attribute *attr, const char *buf,
 				size_t count)
 {
 	struct regulator_dev *rdev = dev_get_drvdata(dev);
diff --git a/drivers/regulator/pfuze100-regulator.c b/drivers/regulator/pfuze100-regulator.c
index 34ca0fb45961..e2aaa2d4e889 100644
--- a/drivers/regulator/pfuze100-regulator.c
+++ b/drivers/regulator/pfuze100-regulator.c
@@ -657,6 +657,21 @@ static int __devexit pfuze100_regulator_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_MX6_INTER_LDO_BYPASS
+int cpu_freq_suspend_in;	/*judge for in suspend or not*/
+static int pfuze100_regulator_suspend(struct platform_device *pdev,
+					pm_message_t state)
+{
+	cpu_freq_suspend_in = 1;
+	return 0;
+}
+
+static int pfuze100_regulator_resume(struct platform_device *pdev)
+{
+	cpu_freq_suspend_in = 0;
+	return 0;
+}
+#endif
 static struct platform_driver pfuze100_regulator_driver = {
 	.driver = {
 		   .name = "pfuze100-regulator",
@@ -664,6 +679,10 @@ static struct platform_driver pfuze100_regulator_driver = {
 		   },
 	.remove = __devexit_p(pfuze100_regulator_remove),
 	.probe = pfuze100_regulator_probe,
+#ifdef CONFIG_MX6_INTER_LDO_BYPASS
+	.suspend = pfuze100_regulator_suspend,
+	.resume = pfuze100_regulator_resume,
+#endif
 };
 
 static int __init pfuze100_regulator_init(void)
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 781e11f5cf34..beba18533371 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -215,6 +215,12 @@ struct imx_port {
 	wait_queue_head_t	dma_wait;
 };
 
+struct imx_port_ucrs {
+	unsigned int	ucr1;
+	unsigned int	ucr2;
+	unsigned int	ucr3;
+};
+
 #ifdef CONFIG_IRDA
 #define USE_IRDA(sport)	((sport)->use_irda)
 #else
@@ -222,6 +228,27 @@ struct imx_port {
 #endif
 
 /*
+ * Save and restore functions for UCR1, UCR2 and UCR3 registers
+ */
+static void imx_port_ucrs_save(struct uart_port *port,
+			       struct imx_port_ucrs *ucr)
+{
+	/* save control registers */
+	ucr->ucr1 = readl(port->membase + UCR1);
+	ucr->ucr2 = readl(port->membase + UCR2);
+	ucr->ucr3 = readl(port->membase + UCR3);
+}
+
+static void imx_port_ucrs_restore(struct uart_port *port,
+				  struct imx_port_ucrs *ucr)
+{
+	/* restore control registers */
+	writel(ucr->ucr1, port->membase + UCR1);
+	writel(ucr->ucr2, port->membase + UCR2);
+	writel(ucr->ucr3, port->membase + UCR3);
+}
+
+/*
  * Handle any change of modem status signal since we were last called.
  */
 static void imx_mctrl_check(struct imx_port *sport)
@@ -553,10 +580,8 @@ static irqreturn_t imx_rxint(int irq, void *dev_id)
 				continue;
 		}
 
-		spin_unlock_irqrestore(&sport->port.lock, flags);
 		if (uart_handle_sysrq_char(&sport->port, (unsigned char)rx))
 			continue;
-		spin_lock_irqsave(&sport->port.lock, flags);
 
 		if (unlikely(rx & URXD_ERR)) {
 			if (rx & URXD_BRK)
@@ -1406,6 +1431,70 @@ imx_verify_port(struct uart_port *port, struct serial_struct *ser)
 	return ret;
 }
 
+#if defined(CONFIG_CONSOLE_POLL)
+static int imx_poll_get_char(struct uart_port *port)
+{
+	struct imx_port_ucrs old_ucr;
+	unsigned int status;
+	unsigned char c;
+
+	/* save control registers */
+	imx_port_ucrs_save(port, &old_ucr);
+
+	/* disable interrupts */
+	writel(UCR1_UARTEN, port->membase + UCR1);
+	writel(old_ucr.ucr2 & ~(UCR2_ATEN | UCR2_RTSEN | UCR2_ESCI),
+	       port->membase + UCR2);
+	writel(old_ucr.ucr3 & ~(UCR3_DCD | UCR3_RI | UCR3_DTREN),
+	       port->membase + UCR3);
+
+	/* poll */
+	do {
+		status = readl(port->membase + USR2);
+	} while (~status & USR2_RDR);
+
+	/* read */
+	c = readl(port->membase + URXD0);
+
+	/* restore control registers */
+	imx_port_ucrs_restore(port, &old_ucr);
+
+	return c;
+}
+
+static void imx_poll_put_char(struct uart_port *port, unsigned char c)
+{
+	struct imx_port_ucrs old_ucr;
+	unsigned int status;
+
+	/* save control registers */
+	imx_port_ucrs_save(port, &old_ucr);
+
+	/* disable interrupts */
+	writel(UCR1_UARTEN, port->membase + UCR1);
+	writel(old_ucr.ucr2 & ~(UCR2_ATEN | UCR2_RTSEN | UCR2_ESCI),
+	       port->membase + UCR2);
+	writel(old_ucr.ucr3 & ~(UCR3_DCD | UCR3_RI | UCR3_DTREN),
+	       port->membase + UCR3);
+
+	/* drain */
+	do {
+		status = readl(port->membase + USR1);
+	} while (~status & USR1_TRDY);
+
+	/* write */
+	writel(c, port->membase + URTX0);
+
+	/* flush */
+	do {
+		status = readl(port->membase + USR2);
+	} while (~status & USR2_TXDC);
+
+	/* restore control registers */
+	imx_port_ucrs_restore(port, &old_ucr);
+}
+#endif
+
 static struct uart_ops imx_pops = {
 	.tx_empty	= imx_tx_empty,
 	.set_mctrl	= imx_set_mctrl,
@@ -1423,6 +1512,10 @@ static struct uart_ops imx_pops = {
 	.request_port	= imx_request_port,
 	.config_port	= imx_config_port,
 	.verify_port	= imx_verify_port,
+#if defined(CONFIG_CONSOLE_POLL)
+	.poll_get_char  = imx_poll_get_char,
+	.poll_put_char  = imx_poll_put_char,
+#endif
 };
 
 static struct imx_port *imx_ports[UART_NR];
@@ -1445,15 +1538,22 @@ static void
 imx_console_write(struct console *co, const char *s, unsigned int count)
 {
 	struct imx_port *sport = imx_ports[co->index];
-	unsigned int old_ucr1, old_ucr2, ucr1;
+	struct imx_port_ucrs old_ucr;
+	unsigned int ucr1;
 	unsigned long flags;
+	int locked = 1;
+
+	local_irq_save(flags);
+	if (sport->port.sysrq)
+		locked = 0;
+	else
+		spin_lock(&sport->port.lock);
 
-	spin_lock_irqsave(&sport->port.lock, flags);
 	/*
-	 *	First, save UCR1/2 and then disable interrupts
+	 *	First, save UCR1/2/3 and then disable interrupts
 	 */
-	ucr1 = old_ucr1 = readl(sport->port.membase + UCR1);
-	old_ucr2 = readl(sport->port.membase + UCR2);
+	imx_port_ucrs_save(&sport->port, &old_ucr);
+	ucr1 = old_ucr.ucr1;
 
 	if (cpu_is_mx1())
 		ucr1 |= MX1_UCR1_UARTCLKEN;
@@ -1462,19 +1562,21 @@ imx_console_write(struct console *co, const char *s, unsigned int count)
 
 	writel(ucr1, sport->port.membase + UCR1);
 
-	writel(old_ucr2 | UCR2_TXEN, sport->port.membase + UCR2);
+	writel(old_ucr.ucr2 | UCR2_TXEN, sport->port.membase + UCR2);
 
 	uart_console_write(&sport->port, s, count, imx_console_putchar);
 
 	/*
 	 *	Finally, wait for transmitter to become empty
-	 *	and restore UCR1/2
+	 *	and restore UCR1/2/3
 	 */
 	while (!(readl(sport->port.membase + USR2) & USR2_TXDC));
 
-	writel(old_ucr1, sport->port.membase + UCR1);
-	writel(old_ucr2, sport->port.membase + UCR2);
-	spin_unlock_irqrestore(&sport->port.lock, flags);
+	imx_port_ucrs_restore(&sport->port, &old_ucr);
+
+	if (locked)
+		spin_unlock(&sport->port.lock);
+	local_irq_restore(flags);
 }
 
 /*
diff --git a/drivers/usb/gadget/zero.c b/drivers/usb/gadget/zero.c
index 5440b0eb71ac..c6e5d054b8bc 100644
--- a/drivers/usb/gadget/zero.c
+++ b/drivers/usb/gadget/zero.c
@@ -107,16 +107,26 @@ module_param(loopdefault, bool, S_IRUGO|S_IWUSR);
 #else
 #define DRIVER_VENDOR_NUM	0x1a0a		/* OTG test device IDs */
 #define DRIVER_PRODUCT_NUM	0xbadd
-#define DEFAULT_AUTORESUME	5
+#define DEFAULT_AUTORESUME	5000
 #endif
 
+static unsigned startms;
+
 /* If the optional "autoresume" mode is enabled, it provides good
  * functional coverage for the "USBCV" test harness from USB-IF.
  * It's always set if OTG mode is enabled.
  */
-unsigned autoresume = DEFAULT_AUTORESUME;
+static unsigned autoresume = DEFAULT_AUTORESUME;
 module_param(autoresume, uint, S_IRUGO);
-MODULE_PARM_DESC(autoresume, "zero, or seconds before remote wakeup");
+MODULE_PARM_DESC(autoresume, "minimum milliseconds before sending remote wakeup");
+
+static unsigned interval;
+module_param(interval, uint, S_IRUGO);
+MODULE_PARM_DESC(interval, "zero, or milliseconds for interval remote wakeup time");
+
+static unsigned endms = 5000;
+module_param(endms, uint, S_IRUGO);
+MODULE_PARM_DESC(endms, "max milliseconds before sending remote wakeup");
 
 /*-------------------------------------------------------------------------*/
 
@@ -250,8 +260,11 @@ static void zero_suspend(struct usb_composite_dev *cdev)
 		return;
 
 	if (autoresume) {
-		mod_timer(&autoresume_timer, jiffies + (HZ * autoresume));
-		DBG(cdev, "suspend, wakeup in %d seconds\n", autoresume);
+		mod_timer(&autoresume_timer, jiffies + msecs_to_jiffies(startms));
+		DBG(cdev, "suspend, wakeup in %d milliseconds\n", startms);
+		startms += interval;
+		if (startms == endms)
+			startms = autoresume;
 	} else
 		DBG(cdev, "%s\n", __func__);
 }
@@ -327,6 +340,10 @@ static int __ref zero_bind(struct usb_composite_dev *cdev)
 		init_utsname()->sysname, init_utsname()->release,
 		gadget->name);
 
+	startms = autoresume;
+	if (autoresume > 0 && autoresume < 5)
+		pr_warning("%s: time before sending remote wakeup is less than 5ms, should not send resume signal.\n",
+			longname);
 	return 0;
 }
 
@@ -336,6 +353,11 @@ static int zero_unbind(struct usb_composite_dev *cdev)
 	return 0;
 }
 
+static void  zero_disconnect(struct usb_composite_dev *cdev)
+{
+	startms = autoresume;
+}
+
 static struct usb_composite_driver zero_driver = {
 	.name		= "zero",
 	.dev		= &device_desc,
@@ -343,6 +365,7 @@ static struct usb_composite_driver zero_driver = {
 	.unbind		= zero_unbind,
 	.suspend	= zero_suspend,
 	.resume		= zero_resume,
+	.disconnect     = zero_disconnect,
 };
 
 MODULE_AUTHOR("David Brownell");
diff --git a/drivers/video/mxc/Kconfig b/drivers/video/mxc/Kconfig
index fe9de7362544..f0afddc95bd3 100644
--- a/drivers/video/mxc/Kconfig
+++ b/drivers/video/mxc/Kconfig
@@ -104,6 +104,17 @@ config FB_MXC_EINK_AUTO_UPDATE_MODE
     default n
     depends on FB_MXC_EINK_PANEL
 
+config FB_MXC_SIPIX_PANEL
+	depends on FB_MXC
+	depends on DMA_ENGINE
+	select FB_DEFERRED_IO
+	tristate "SIPIX Panel Framebuffer"
+
+config FB_MXC_SIPIX_AUTO_UPDATE_MODE
+	bool "SIPIX Auto-update Mode Support"
+	default n
+	depends on FB_MXC_SIPIX_PANEL
+
 config FB_MXC_ELCDIF_FB
 	depends on FB && ARCH_MXC
 	tristate "Support MXC ELCDIF framebuffer"
diff --git a/drivers/video/mxc/Makefile b/drivers/video/mxc/Makefile
index 084837c70929..21aef052ecd6 100644
--- a/drivers/video/mxc/Makefile
+++ b/drivers/video/mxc/Makefile
@@ -25,4 +25,5 @@ obj-$(CONFIG_FB_MXC_SEIKO_WVGA_SYNC_PANEL)   += mxcfb_seiko_wvga.o
 obj-$(CONFIG_FB_MXC_TVOUT_CH7024)           += ch7024.o
 obj-$(CONFIG_FB_MXC_CH7026)		    		+= mxcfb_ch7026.o
 obj-$(CONFIG_FB_MXC_EINK_PANEL)             += mxc_epdc_fb.o
+obj-$(CONFIG_FB_MXC_SIPIX_PANEL)            += mxc_spdc_fb.o
 obj-$(CONFIG_FB_MXC_ELCDIF_FB)		    += mxc_elcdif_fb.o
diff --git a/drivers/video/mxc/epdc_regs.h b/drivers/video/mxc/epdc_regs.h
index 50d352e7a212..cc7b5000db18 100644
--- a/drivers/video/mxc/epdc_regs.h
+++ b/drivers/video/mxc/epdc_regs.h
@@ -163,9 +163,7 @@ extern void __iomem *epdc_base;
 #define EPDC_PIGEON_15_0		(epdc_base + 0xBC0)
 #define EPDC_PIGEON_15_1		(epdc_base + 0xBD0)
 #define EPDC_PIGEON_15_2		(epdc_base + 0xBE0)
-#define EPDC_PIGEON_16_0		(epdc_base + 0xC00)
-#define EPDC_PIGEON_16_1		(epdc_base + 0xC10)
-#define EPDC_PIGEON_16_2		(epdc_base + 0xC20)
+#define EPDC_WB_ADDR_TCE		(epdc_base + 0xC10)
 
 /*
  * Register field definitions
diff --git a/drivers/video/mxc/ldb.c b/drivers/video/mxc/ldb.c
index 493abd662036..8b84498fd285 100644
--- a/drivers/video/mxc/ldb.c
+++ b/drivers/video/mxc/ldb.c
@@ -457,6 +457,17 @@ static int ldb_disp_init(struct mxc_dispdrv_handle *disp,
 		else
 			ldb->mode = plat_data->mode;
 
+		if ((ldb->mode == LDB_SIN0) || (ldb->mode == LDB_SIN1)) {
+			ret = ldb->mode - LDB_SIN0;
+			if (plat_data->disp_id != ret) {
+				dev_warn(&ldb->pdev->dev,
+					"change IPU DI%d to IPU DI%d for LDB "
+					"channel%d.\n",
+					plat_data->disp_id, ret, ret);
+				plat_data->disp_id = ret;
+			}
+		}
+
 		if (ldb->mode == LDB_SPL_DI0) {
 			reg |= LDB_SPLIT_MODE_EN | LDB_CH0_MODE_EN_TO_DI0
 				| LDB_CH1_MODE_EN_TO_DI0;
diff --git a/drivers/video/mxc/mxc_edid.c b/drivers/video/mxc/mxc_edid.c
index e8a2b9761b65..f86313df7dea 100644
--- a/drivers/video/mxc/mxc_edid.c
+++ b/drivers/video/mxc/mxc_edid.c
@@ -350,6 +350,16 @@ int mxc_edid_parse_ext_blk(unsigned char *edid,
 					break;
 				}
 			case 0x4: /*Speaker allocation block*/
+				{
+					i = 0;
+					while (i < blklen) {
+						cfg->speaker_alloc = edid[index + 1];
+						index += 3;
+						i += 3;
+						DPRINTK("Speaker Alloc           : %02x\n", cfg->speaker_alloc);
+					}
+					break;
+				}
 			case 0x7: /*User extended block*/
 			default:
 				/* skip */
diff --git a/drivers/video/mxc/mxc_elcdif_fb.c b/drivers/video/mxc/mxc_elcdif_fb.c
index 7a5fa6d1900c..2c96eecc111f 100644
--- a/drivers/video/mxc/mxc_elcdif_fb.c
+++ b/drivers/video/mxc/mxc_elcdif_fb.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2010-2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2010-2012 Freescale Semiconductor, Inc.
  */
 
 /*
@@ -128,8 +128,10 @@ static inline void setup_dotclk_panel(u32 pixel_clk,
 	__raw_writel(BM_ELCDIF_CTRL_SHIFT_NUM_BITS,
 		     elcdif_base + HW_ELCDIF_CTRL_CLR);
 
+	__raw_writel(BM_ELCDIF_CTRL2_OUTSTANDING_REQS,
+		     elcdif_base + HW_ELCDIF_CTRL2_CLR);
 	__raw_writel(BF_ELCDIF_CTRL2_OUTSTANDING_REQS
-		    (BV_ELCDIF_CTRL2_OUTSTANDING_REQS__REQ_8),
+		    (BV_ELCDIF_CTRL2_OUTSTANDING_REQS__REQ_16),
 		     elcdif_base + HW_ELCDIF_CTRL2_SET);
 
 	/* Recover on underflow */
@@ -824,7 +826,7 @@ static int mxc_elcdif_fb_set_par(struct fb_info *fbi)
 	mxc_init_elcdif();
 	mxc_elcdif_init_panel();
 
-	dev_dbg(fbi->device, "pixclock = %ul Hz\n",
+	dev_dbg(fbi->device, "pixclock = %lu Hz\n",
 		(u32) (PICOS2KHZ(fbi->var.pixclock) * 1000UL));
 
 	memset(&sig_cfg, 0, sizeof(sig_cfg));
@@ -1369,7 +1371,7 @@ static int mxc_elcdif_fb_suspend(struct platform_device *pdev,
 	struct mxc_elcdif_fb_data *data = (struct mxc_elcdif_fb_data *)fbi->par;
 	int saved_blank;
 
-	acquire_console_sem();
+	console_lock();
 	fb_set_suspend(fbi, 1);
 	saved_blank = data->cur_blank;
 	mxc_elcdif_fb_blank(FB_BLANK_POWERDOWN, fbi);
@@ -1388,7 +1390,7 @@ static int mxc_elcdif_fb_suspend(struct platform_device *pdev,
 		clk_disable(g_elcdif_axi_clk);
 		g_elcdif_axi_clk_enable = false;
 	}
-	release_console_sem();
+	console_unlock();
 	return 0;
 }
 
@@ -1397,10 +1399,10 @@ static int mxc_elcdif_fb_resume(struct platform_device *pdev)
 	struct fb_info *fbi = platform_get_drvdata(pdev);
 	struct mxc_elcdif_fb_data *data = (struct mxc_elcdif_fb_data *)fbi->par;
 
-	acquire_console_sem();
+	console_lock();
 	mxc_elcdif_fb_blank(data->next_blank, fbi);
 	fb_set_suspend(fbi, 0);
-	release_console_sem();
+	console_unlock();
 
 	return 0;
 }
diff --git a/drivers/video/mxc/mxc_epdc_fb.c b/drivers/video/mxc/mxc_epdc_fb.c
index da77373e03a6..ef4916db0436 100644
--- a/drivers/video/mxc/mxc_epdc_fb.c
+++ b/drivers/video/mxc/mxc_epdc_fb.c
@@ -1130,6 +1130,7 @@ static void epdc_init_sequence(struct mxc_epdc_fb_data *fb_data)
 	epdc_init_settings(fb_data);
 	__raw_writel(fb_data->waveform_buffer_phys, EPDC_WVADDR);
 	__raw_writel(fb_data->working_buffer_phys, EPDC_WB_ADDR);
+	__raw_writel(fb_data->working_buffer_phys, EPDC_WB_ADDR_TCE);
 	fb_data->in_init = true;
 	epdc_powerup(fb_data);
 	draw_mode0(fb_data);
@@ -4006,8 +4007,8 @@ static void mxc_epdc_fb_fw_handler(const struct firmware *fw,
 	clk_enable(fb_data->epdc_clk_pix);
 	rounded_pix_clk = clk_round_rate(fb_data->epdc_clk_pix, target_pix_clk);
 
-	if (((rounded_pix_clk >= target_pix_clk + target_pix_clk/200) ||
-		(rounded_pix_clk <= target_pix_clk - target_pix_clk/200))) {
+	if (((rounded_pix_clk >= target_pix_clk + target_pix_clk/100) ||
+		(rounded_pix_clk <= target_pix_clk - target_pix_clk/100))) {
 		/* Can't get close enough without changing parent clk */
 		epdc_parent = clk_get_parent(fb_data->epdc_clk_pix);
 		rounded_parent_rate = clk_round_rate(epdc_parent, target_pix_clk);
@@ -4018,8 +4019,8 @@ static void mxc_epdc_fb_fw_handler(const struct firmware *fw,
 		clk_set_rate(epdc_parent, epdc_pix_rate);
 
 		rounded_pix_clk = clk_round_rate(fb_data->epdc_clk_pix, target_pix_clk);
-		if (((rounded_pix_clk >= target_pix_clk + target_pix_clk/200) ||
-			(rounded_pix_clk <= target_pix_clk - target_pix_clk/200)))
+		if (((rounded_pix_clk >= target_pix_clk + target_pix_clk/100) ||
+			(rounded_pix_clk <= target_pix_clk - target_pix_clk/100)))
 			/* Still can't get a good clock, provide warning */
 			dev_err(fb_data->dev, "Unable to get an accurate EPDC pix clk"
 				"desired = %lu, actual = %lu\n", target_pix_clk,
diff --git a/drivers/video/mxc/mxc_ipuv3_fb.c b/drivers/video/mxc/mxc_ipuv3_fb.c
index 11bccff413a8..348bd818ee69 100644
--- a/drivers/video/mxc/mxc_ipuv3_fb.c
+++ b/drivers/video/mxc/mxc_ipuv3_fb.c
@@ -316,15 +316,6 @@ static int mxcfb_set_par(struct fb_info *fbi)
 
 	dev_dbg(fbi->device, "Reconfiguring framebuffer\n");
 
-	if (mxc_fbi->dispdrv && mxc_fbi->dispdrv->drv->setup) {
-		retval = mxc_fbi->dispdrv->drv->setup(mxc_fbi->dispdrv, fbi);
-		if (retval < 0) {
-			dev_err(fbi->device, "setup error, dispdrv:%s.\n",
-					mxc_fbi->dispdrv->drv->name);
-			return -EINVAL;
-		}
-	}
-
 	ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
 	ipu_disable_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_irq);
 	ipu_clear_irq(mxc_fbi->ipu, mxc_fbi->ipu_ch_nf_irq);
@@ -391,6 +382,15 @@ static int mxcfb_set_par(struct fb_info *fbi)
 	if (mxc_fbi->next_blank != FB_BLANK_UNBLANK)
 		return retval;
 
+	if (mxc_fbi->dispdrv && mxc_fbi->dispdrv->drv->setup) {
+		retval = mxc_fbi->dispdrv->drv->setup(mxc_fbi->dispdrv, fbi);
+		if (retval < 0) {
+			dev_err(fbi->device, "setup error, dispdrv:%s.\n",
+					mxc_fbi->dispdrv->drv->name);
+			return -EINVAL;
+		}
+	}
+
 	_setup_disp_channel1(fbi);
 
 	if (!mxc_fbi->overlay) {
diff --git a/drivers/video/mxc/mxc_spdc_fb.c b/drivers/video/mxc/mxc_spdc_fb.c
new file mode 100644
index 000000000000..37938b9db712
--- /dev/null
+++ b/drivers/video/mxc/mxc_spdc_fb.c
@@ -0,0 +1,4168 @@
+/*
+ * Copyright (C) 2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+ *
+ */
+/*
+ * Based on MXC EPDC Driver, Freescale Solutions, Inc All Rights Reserved.
+ */
+
+#include "mxc_spdc_fb.h"
+
+#define MERGE_OK	0
+#define MERGE_FAIL      1
+#define MERGE_BLOCK     2
+
+#define SPDC_DEFAULT_TEMP	30
+#define TEMP_NO_SET		0xFF
+#define POWER_STATE_OFF		0
+#define POWER_STATE_ON		1
+#define POWER_READY_OFF		false
+#define POWER_READY_ON		true
+
+#define INIT_UPDATE_MARKER	0x12345678
+#define PAN_UPDATE_MARKER	0x12345679
+
+#define SPDC_MAX_NUM_UPDATES	32
+#define SPDC_MAX_NUM_BUFFERS	2
+#define SPDC_MAX_NUM_PREPROCESS	15
+#define NUM_SCREENS_MIN		2
+#define SPDC_DEFAULT_BPP	16
+
+mxc_spdc_t *g_fb_data;
+
+static int mxc_spdc_fb_send_update(struct mxcfb_update_data *upd_data,
+				   struct fb_info *info);
+static int mxc_spdc_fb_wait_update_complete(struct mxcfb_update_marker_data
+		*marker_data, struct fb_info *info);
+
+static const struct mxc_spdc_resolution_map_para spdc_gray_res_map[] = {
+/*define Gray Mode resolution mapping*/
+	{0x18, 600, 800, PORTRAIT},
+	{0x19, 768, 1024, PORTRAIT},
+	{0x1a, 0,   0,    RESERVED},
+	{0x1b, 600, 1024, PORTRAIT},
+	{0x1c, 825, 1200, PORTRAIT},
+	{0x1d, 1024, 1280, PORTRAIT},
+	{0x1e, 1200, 1600, PORTRAIT},
+	{0x10, 800, 1024, PORTRAIT},
+	{0x11, 825, 1280, PORTRAIT},
+	{0x12, 800, 1280, PORTRAIT},
+	{0x13, 768, 1280, PORTRAIT},
+	{0x14, 960, 1280, PORTRAIT},
+	{0x0, 800, 600, LANDSCAPE},
+	{0x1, 1024, 768, LANDSCAPE},
+	{0x2, 0, 0, RESERVED},
+	{0x3, 1024, 600, LANDSCAPE},
+	{0x4, 1200, 825, LANDSCAPE},
+	{0x5, 1280, 1024, LANDSCAPE},
+	{0x6, 1600, 1200, LANDSCAPE},
+	{0x7, 1024, 800, LANDSCAPE},
+	{0x8, 1280, 825, LANDSCAPE},
+	{0x9, 1280, 800, LANDSCAPE},
+	{0xa, 1280, 768, LANDSCAPE},
+	{0xb, 1280, 960, LANDSCAPE},
+	{0xFFFF, 800, 600, LANDSCAPE},
+};
+
+static const struct mxc_spdc_resolution_map_para spdc_rgbw_res_map[] = {
+/*define RGBW Mode resolution mapping*/
+	{0x18, 300, 400, PORTRAIT},
+	{0x19, 384, 512, PORTRAIT},
+	{0x1a, 0,   0,    RESERVED},
+	{0x1b, 300, 512, PORTRAIT},
+	{0x1c, 0, 0, RESERVED},
+	{0x1d, 512, 640, PORTRAIT},
+	{0x1e, 600, 800, PORTRAIT},
+	{0x10, 400, 512, PORTRAIT},
+	{0x11, 0, 0, RESERVED},
+	{0x12, 400, 640, PORTRAIT},
+	{0x13, 384, 640, PORTRAIT},
+	{0x14, 480, 640, PORTRAIT},
+	{0x0, 400, 300, LANDSCAPE},
+	{0x1, 512, 384, LANDSCAPE},
+	{0x2, 0, 0, RESERVED},
+	{0x3, 512, 300, LANDSCAPE},
+	{0x4, 0, 0, RESERVED},
+	{0x5, 640, 512, LANDSCAPE},
+	{0x6, 800, 600, LANDSCAPE},
+	{0x7, 512, 400, LANDSCAPE},
+	{0x8, 0, 0, RESERVED},
+	{0x9, 640, 400, LANDSCAPE},
+	{0xa, 640, 384, LANDSCAPE},
+	{0xb, 640, 480, LANDSCAPE},
+	{0xFFFF, 400, 300, LANDSCAPE},
+};
+
+static void get_panel_init_set(struct imx_spdc_panel_init_set*
+					panel_set, u32 *val)
+{
+	*val = panel_set->yoe_pol |
+		(panel_set->dual_gate << 1) |
+		(panel_set->ud << 7) |
+		(panel_set->rl << 8) |
+		(panel_set->data_filter_n  << 9) |
+		(panel_set->power_ready  << 10) |
+		(panel_set->rgbw_mode_enable  << 11) |
+		(panel_set->hburst_len_en << 13) |
+		((panel_set->resolution & 0x1F) << 2);
+}
+
+static inline void spdc_intr_enable(mxc_spdc_t *fb_data, u32 int_type)
+{
+	u32 status;
+
+	status = __raw_readl(fb_data->hwp + SPDC_INT_ENABLE);
+	status |= (int_type & SPDC_IRQ_ALL_MASK);
+	__raw_writel(status, fb_data->hwp + SPDC_INT_ENABLE);
+}
+
+static bool spdc_is_update_finish(mxc_spdc_t *fb_data)
+{
+	u32 val =  __raw_readl(fb_data->hwp + SPDC_INT_STA_CLR);
+	bool is_finish = (val & SPDC_IRQ_STA_FRAME_UPDATE) ? true : false;
+
+	return is_finish;
+}
+
+static int spdc_get_intr_stat(mxc_spdc_t *fb_data)
+{
+	u32 status = __raw_readl(fb_data->hwp + SPDC_INT_STA_CLR);
+	return status & 0xF;
+}
+
+static inline void
+spdc_intr_stat_clear(mxc_spdc_t *fb_data, u32 int_type)
+{
+	/* write 1 to clear status */
+	u32 status = (int_type & SPDC_IRQ_STA_ALL_MASK);
+	__raw_writel(status, fb_data->hwp + SPDC_INT_STA_CLR);
+}
+
+static inline void spdc_set_nextbuf_addr(mxc_spdc_t *fb_data)
+{
+	u32 addr = fb_data->fresh_param.buf_addr.next_buf_phys_addr;
+	__raw_writel(addr, fb_data->hwp + SPDC_NEXT_BUF);
+	dev_dbg(fb_data->dev, "add: 0x%x\n", addr);
+}
+
+static inline void spdc_set_curbuf_addr(mxc_spdc_t *fb_data)
+{
+	u32 addr = fb_data->fresh_param.buf_addr.cur_buf_phys_addr;
+	__raw_writel(addr, fb_data->hwp + SPDC_CURRENT_BUF);
+}
+
+static inline void spdc_set_prebuf_addr(mxc_spdc_t *fb_data)
+{
+	u32 addr = fb_data->fresh_param.buf_addr.pre_buf_phys_addr;
+	__raw_writel(addr, fb_data->hwp + SPDC_PRE_BUF);
+}
+
+static inline void spdc_set_cntbuf_addr(mxc_spdc_t *fb_data)
+{
+	u32 addr = fb_data->fresh_param.buf_addr.frm_cnt_buf_phys_addr;
+	__raw_writel(addr, fb_data->hwp + SPDC_CNT_BUF);
+}
+
+static inline void spdc_set_lutbuf_addr(mxc_spdc_t *fb_data)
+{
+	u32 addr = fb_data->fresh_param.buf_addr.lut_buf_phys_addr;
+	__raw_writel(addr, fb_data->hwp + SPDC_LUT_BUF);
+}
+
+static inline void spdc_set_update_coord(mxc_spdc_t *fb_data)
+{
+	u32 x = fb_data->fresh_param.update_region.left;
+	u32 y = fb_data->fresh_param.update_region.top;
+
+	if (!x)
+		x++;
+	if (!y)
+		y++;
+
+	dev_dbg(fb_data->dev, "x:%d, y:%d\n", x, y);
+	x = (u32)(((x & SPDC_UPDATE_X_Y_MAX_SIZE) << 16) |
+			(y & SPDC_UPDATE_X_Y_MAX_SIZE));
+	__raw_writel(x, fb_data->hwp + SPDC_UPDATA_X_Y);
+}
+
+static inline void spdc_set_update_dimensions(mxc_spdc_t *fb_data)
+{
+	u32 w = fb_data->fresh_param.update_region.width;
+	u32 h = fb_data->fresh_param.update_region.height;
+
+	if (!w)
+		w++;
+	if (!h)
+		h++;
+
+	dev_dbg(fb_data->dev, "w:%d, h:%d\n", w, h);
+	w = (u32)(((w & SPDC_UPDATE_W_H_MAX_SIZE) << 16) |
+			(h & SPDC_UPDATE_W_H_MAX_SIZE));
+	__raw_writel(w, fb_data->hwp + SPDC_UPDATE_W_H);
+}
+
+static inline void spdc_set_update_temper(mxc_spdc_t *fb_data)
+{
+	s8 temper = (s8)(fb_data->fresh_param.temper & 0xFF) << 1;
+
+	if (temper > -110 && temper < 200)
+		__raw_writel(temper, fb_data->hwp + SPDC_TEMP_INFO);
+	else
+		__raw_writel(SPDC_DEFAULT_TEMP, fb_data->hwp + SPDC_TEMP_INFO);
+}
+
+static inline void spdc_trigger_update(mxc_spdc_t *fb_data)
+{
+	u32 val;
+	struct partial_refresh_param *fresh_param = &fb_data->fresh_param;
+
+	if ((fresh_param->wave_mode & SPDC_WAV_MODE_MASK) && fresh_param->flash)
+		val = SPDC_DISP_TRIGGER_FLASH;
+	else
+		val = 0;
+
+	val |= fresh_param->wave_mode << 1;
+	val |= SPDC_DISP_TRIGGER_ENABLE;
+	dev_dbg(fb_data->dev, "wave:%d\n", fresh_param->wave_mode);
+	__raw_writel(val, fb_data->hwp + SPDC_DISP_TRIGGER);
+}
+
+static bool is_lut_checksum_ok(mxc_spdc_t *fb_data)
+{
+	u32 status;
+
+	status = __raw_readl(fb_data->hwp + SPDC_STATUS);
+	status &= SPDC_IRQ_STA_ERR;
+
+	return status ? true : false;
+}
+
+static void spdc_clk_gate(mxc_spdc_t *fb_data, bool enable)
+{
+	if (enable)
+		__raw_writel(SPDC_SW_GATE_CLK_ENABLE,
+		fb_data->hwp + SPDC_SW_GATE_CLK);
+	else
+		__raw_writel(~SPDC_SW_GATE_CLK_ENABLE,
+		fb_data->hwp + SPDC_SW_GATE_CLK);
+}
+
+static int update_panel_init_set(mxc_spdc_t *fb_data)
+{
+	int ret = 0;
+	u32 init_val;
+
+	get_panel_init_set(&fb_data->panel_set, &init_val);
+	dev_dbg(fb_data->dev, "panel init setting:%x\n", init_val);
+
+	__raw_writel(init_val, fb_data->hwp + SPDC_PANEL_INIT_SET);
+
+	/*wait init setting update finish*/
+	ret = wait_for_completion_timeout(&fb_data->init_finish,
+				msecs_to_jiffies(4000));
+	if (!ret)
+		dev_err(fb_data->dev, "Timed out for init setting!\n");
+
+	return ret;
+}
+
+static void spdc_panel_pwr_on(mxc_spdc_t *fb_data)
+{
+	fb_data->panel_set.power_ready = POWER_READY_ON;
+}
+
+static void spdc_panel_pwr_down(mxc_spdc_t *fb_data)
+{
+	fb_data->panel_set.power_ready = POWER_READY_OFF;
+}
+
+static void spdc_powerdown(mxc_spdc_t *fb_data)
+{
+	mutex_lock(&fb_data->power_mutex);
+
+	/* If powering_down has been cleared, a powerup
+	* request is pre-empting this powerdown request.
+	*/
+	if (!fb_data->powering_down
+		|| (fb_data->power_state == POWER_STATE_OFF)) {
+		mutex_unlock(&fb_data->power_mutex);
+		return;
+	}
+
+	dev_dbg(fb_data->dev, "spdc Powerdown\n");
+
+	/* Disable power to the AUO panel */
+	regulator_disable(fb_data->vcom_regulator);
+	regulator_disable(fb_data->display_regulator);
+
+	/*enable spdc clock gating*/
+	spdc_clk_gate(fb_data, true);
+	clk_disable(fb_data->spdc_clk_pix);
+	clk_disable(fb_data->spdc_clk_axi);
+
+	/* Disable pins used by SPDC (to prevent leakage current) */
+	if (fb_data->pdata->disable_pins)
+		fb_data->pdata->disable_pins();
+
+	/* turn off the V3p3 */
+	regulator_disable(fb_data->v3p3_regulator);
+
+	fb_data->power_state = POWER_STATE_OFF;
+	fb_data->powering_down = false;
+	spdc_panel_pwr_down(fb_data);
+
+	if (fb_data->wait_for_powerdown) {
+		fb_data->wait_for_powerdown = false;
+		complete(&fb_data->powerdown_compl);
+	}
+
+	mutex_unlock(&fb_data->power_mutex);
+}
+
+static void spdc_powerup(mxc_spdc_t *fb_data)
+{
+	int ret = 0;
+	mutex_lock(&fb_data->power_mutex);
+
+	/*
+	 * If power down request is pending, clear
+	 * powering_down to cancel the request.
+	 */
+	if (fb_data->powering_down)
+		fb_data->powering_down = false;
+
+	if (fb_data->power_state == POWER_STATE_ON) {
+		mutex_unlock(&fb_data->power_mutex);
+		return;
+	}
+
+	dev_dbg(fb_data->dev, "spdc Powerup\n");
+
+	/* Enable the v3p3 regulator */
+	ret = regulator_enable(fb_data->v3p3_regulator);
+	if (IS_ERR((void *)ret)) {
+		dev_err(fb_data->dev, "Unable to enable V3P3 regulator."
+			"err = 0x%x\n", ret);
+		mutex_unlock(&fb_data->power_mutex);
+		return;
+	}
+
+	msleep(1);
+
+	/* Enable pins used by SPDC */
+	if (fb_data->pdata->enable_pins)
+		fb_data->pdata->enable_pins();
+
+	/* Enable clocks to SPDC */
+	clk_enable(fb_data->spdc_clk_axi);
+	clk_enable(fb_data->spdc_clk_pix);
+
+	/*disable spdc gate*/
+	spdc_clk_gate(fb_data, false);
+
+	/* Enable power to the EPD panel */
+	ret = regulator_enable(fb_data->display_regulator);
+	if (IS_ERR((void *)ret)) {
+		dev_err(fb_data->dev, "Unable to enable DISPLAY regulator."
+			"err = 0x%x\n", ret);
+		mutex_unlock(&fb_data->power_mutex);
+		return;
+	}
+	ret = regulator_enable(fb_data->vcom_regulator);
+	if (IS_ERR((void *)ret)) {
+		dev_err(fb_data->dev, "Unable to enable VCOM regulator."
+			"err = 0x%x\n", ret);
+		mutex_unlock(&fb_data->power_mutex);
+		return;
+	}
+
+	fb_data->power_state = POWER_STATE_ON;
+	spdc_panel_pwr_on(fb_data);
+
+	mutex_unlock(&fb_data->power_mutex);
+}
+
+#ifdef DEBUG
+static void
+check_waveform(u32 *wv_buf_orig, u32 *wv_buf_cur, u32 wv_buf_size)
+{
+	int i;
+	bool is_mismatch = false;
+	for (i = 0; i < wv_buf_size; i++) {
+		if (wv_buf_orig[i] != wv_buf_cur[i]) {
+			is_mismatch = true;
+			printk(KERN_ERR "Waveform mismatch!\n");
+		}
+	}
+
+	if (!is_mismatch)
+		printk(KERN_DEBUG "No mismatches!\n");
+}
+#else
+static void
+check_waveform(u32 *wv_buf_orig, u32 *wv_buf_cur, u32 wv_buf_size) {}
+#endif
+
+static void get_spdc_version(mxc_spdc_t *fb_data)
+{
+	struct mxc_spdc_version *spdc_ver = &fb_data->spdc_ver;
+	u32 disp_id, tcon_id;
+
+	disp_id = __raw_readl(fb_data->hwp + SPDC_DISP_VER);
+	tcon_id = __raw_readl(fb_data->hwp + SPDC_TCON_VER);
+
+	spdc_ver->disp_ver.product_id = disp_id & 0xFFFF;
+	spdc_ver->disp_ver.lut_ver = (disp_id >> 16) & 0xFF;
+	spdc_ver->disp_ver.epd_type = (disp_id >> 24) & 0xFF;
+	spdc_ver->tcon_ver = tcon_id & 0xFF;
+
+	dev_info(fb_data->dev, "EPD type ID:%x, Tcon ID:%x\n",
+		spdc_ver->disp_ver.product_id, spdc_ver->tcon_ver);
+}
+
+static void spdc_set_update_concurrency(mxc_spdc_t *fb_data)
+{
+	u32 concur_mode;
+
+	concur_mode = fb_data->fresh_param.concur & 0xFF;
+	concur_mode |= (SPDC_LUT_MODE_OFFSET << 8);
+
+	__raw_writel(concur_mode, fb_data->hwp + SPDC_LUT_PARA_UPDATE);
+}
+
+static bool is_preprocess_list_full(mxc_spdc_t *fb_data)
+{
+	/* Check to see if preprocess are full in this list */
+	if (fb_data->upd_preprocess_num >= SPDC_MAX_NUM_PREPROCESS)
+		return true;
+	else
+		return false;
+}
+
+static void spdc_submit_update(mxc_spdc_t *fb_data)
+{
+	fb_data->updates_active = true;
+
+	spdc_set_nextbuf_addr(fb_data);
+	spdc_set_update_coord(fb_data);
+	spdc_set_update_dimensions(fb_data);
+	spdc_set_update_temper(fb_data);
+	spdc_trigger_update(fb_data);
+}
+
+static int spdc_init_sequence(mxc_spdc_t *fb_data)
+{
+	struct fb_var_screeninfo *screeninfo = &fb_data->spdc_fb_var;
+	struct spdc_buffer_addr *buf_addr = &fb_data->fresh_param.buf_addr;
+	struct imx_spdc_panel_init_set *init_set =
+		fb_data->pdata->spdc_mode->init_set;
+	u32 xres, yres;
+	int ret = -EFAULT;
+
+	/*init spdc power*/
+	spdc_powerup(fb_data);
+
+	/* enable all interrupt */
+	spdc_intr_stat_clear(fb_data, SPDC_INT_STA_CLR);
+	spdc_intr_enable(fb_data, SPDC_IRQ_ALL_MASK);
+	/* set ACC concurrency update mode */
+	if (fb_data->fresh_param.concur)
+		spdc_set_update_concurrency(fb_data);
+
+	/* program SPDC register and trigger to process buffer*/
+	buf_addr->next_buf_phys_addr = fb_data->phy_next_buf;
+	buf_addr->cur_buf_phys_addr = fb_data->phy_current_buf;
+	buf_addr->pre_buf_phys_addr = fb_data->phy_pre_buf;
+	buf_addr->frm_cnt_buf_phys_addr = fb_data->phy_cnt_buf;
+	buf_addr->lut_buf_phys_addr = fb_data->phy_lut_buf;
+
+	/* Use unrotated (native) width/height */
+	if ((screeninfo->rotate == FB_ROTATE_CW) ||
+		(screeninfo->rotate == FB_ROTATE_CCW)) {
+		xres = screeninfo->yres;
+		yres = screeninfo->xres;
+	} else {
+		xres = screeninfo->xres;
+		yres = screeninfo->yres;
+	}
+	fb_data->fresh_param.update_region.left = 0;
+	fb_data->fresh_param.update_region.top = 0;
+	fb_data->fresh_param.update_region.width = xres;
+	fb_data->fresh_param.update_region.height = yres;
+
+	/* set panel temperature as environment temperature */
+	fb_data->fresh_param.temper = SPDC_DEFAULT_TEMP;
+	/* set waveform mode */
+	fb_data->fresh_param.wave_mode = SPDC_WAV_MODE_DEFAULT;
+
+	spdc_set_update_coord(fb_data);
+	spdc_set_update_dimensions(fb_data);
+
+	spdc_set_update_temper(fb_data);
+
+	spdc_set_nextbuf_addr(fb_data);
+	spdc_set_curbuf_addr(fb_data);
+	spdc_set_prebuf_addr(fb_data);
+	spdc_set_cntbuf_addr(fb_data);
+
+	/* load waveform*/
+	spdc_set_lutbuf_addr(fb_data);
+	ret = wait_for_completion_timeout(&fb_data->lut_down,
+					msecs_to_jiffies(4000));
+	if (!ret) {
+		dev_err(fb_data->dev,
+			"Timed out for lut!\n");
+		return ret;
+	}
+
+	/* init SPDC setting, the setting get from platform data */
+	fb_data->panel_set.yoe_pol = init_set->yoe_pol;
+	fb_data->panel_set.dual_gate = init_set->dual_gate;
+	fb_data->panel_set.ud = init_set->ud;
+	fb_data->panel_set.rl = init_set->rl;
+	fb_data->panel_set.data_filter_n = init_set->data_filter_n;
+	fb_data->panel_set.rgbw_mode_enable = init_set->rgbw_mode_enable;
+	fb_data->panel_set.hburst_len_en = init_set->hburst_len_en;
+	ret = update_panel_init_set(fb_data);
+
+	return ret;
+}
+
+static u32 mxc_spdc_partial_refresh_low(mxc_spdc_t *fb_data, void *buffer)
+{
+	u8 *fresh_addr;
+	void *pattern = buffer;
+	struct partial_refresh_param *fresh_param = &fb_data->fresh_param;
+	u32 fresh_size;
+	int ret = 0;
+
+	fb_data->updates_active = true;
+
+	fresh_addr = (u8 *)(fb_data->virt_start) +
+		(fresh_param->update_region.top * fresh_param->stride) +
+		((fresh_param->update_region.left * fb_data->default_bpp) >> 3);
+	fresh_size = (u32)((fresh_param->update_region.width *
+		fresh_param->update_region.height * fb_data->default_bpp) >> 3);
+
+	if (buffer != NULL) {
+		while (fresh_size > 0) {
+			memcpy((void *)fresh_addr, pattern,
+				fresh_param->update_region.width);
+			fresh_size -= fresh_param->update_region.width;
+			fresh_addr += fresh_param->update_region.top *
+					fresh_param->stride;
+			pattern +=  fresh_param->update_region.width;
+		}
+	}
+
+	/* program SPDC register and trigger to process buffer*/
+	fb_data->fresh_param.buf_addr.next_buf_phys_addr =
+		fb_data->info.fix.smem_start + fb_data->fb_offset;
+	fb_data->fresh_param.wave_mode = fb_data->wv_modes.mode_init;
+	spdc_submit_update(fb_data);
+
+	ret = wait_for_completion_timeout(&fb_data->update_finish,
+					msecs_to_jiffies(3000));
+	if (!ret) {
+		dev_err(fb_data->dev,
+			"display update timeout!\n");
+		return -ETIMEDOUT;
+	}
+
+	return ret;
+}
+
+static u32 spdc_fb_dev_init(mxc_spdc_t *fb_data)
+{
+	fb_data->auto_mode = AUTO_UPDATE_MODE_REGION_MODE;
+	fb_data->fresh_param.wave_mode = SPDC_WAV_MODE_0;
+	fb_data->operation_mode = SPDC_NO_OPERATION;
+	fb_data->is_deep_fresh = false;
+
+	/* Init the concurrency update */
+	fb_data->fresh_param.concur = 0;
+	fb_data->upd_preprocess_num = 0;
+	fb_data->submit_upd_sta = 0;
+
+	fb_data->fresh_param.temper = SPDC_DEFAULT_TEMP;
+
+	return 0;
+}
+
+/**
+ * mxc_spdc_device_is_busy - check spdc device busy status.
+ * Returns 0 if spdc device is idle.
+ */
+static int mxc_spdc_device_is_busy(mxc_spdc_t *fb_data)
+{
+	u32 status;
+	u32 orig_jiffies = jiffies;
+
+	while (1) {
+		status = __raw_readl(fb_data->hwp + SPDC_STATUS);
+		if ((status & SPDC_PANEL_STAUTS_BUSY) &&
+			((status & 0xF0) == SPDC_TCON_STATUS_IDLE))
+			break;
+
+		if (signal_pending(current)) {
+			dev_dbg(fb_data->dev, "SPDC Interrupted\n");
+			return -EINTR;
+		}
+
+		if (time_after(jiffies, orig_jiffies +
+				msecs_to_jiffies(3000))) {
+			dev_dbg(fb_data->dev, "SPDC is busy\n");
+			 return -ETIMEDOUT;
+		}
+
+		schedule();
+	}
+
+	return 0;
+}
+
+static bool is_free_list_full(mxc_spdc_t *fb_data)
+{
+	int count = 0;
+	struct update_data_list *plist;
+
+	/* Count buffers in free buffer list */
+	list_for_each_entry(plist, &fb_data->upd_buf_free_list, list)
+		count++;
+
+	/* Check to see if all buffers are in this list */
+	if (count == fb_data->max_num_updates)
+		return true;
+	else
+		return false;
+}
+
+static void spdc_draw_mode0(mxc_spdc_t *fb_data)
+{
+	struct mxcfb_update_data update;
+	struct mxcfb_update_marker_data upd_marker_data;
+	struct fb_var_screeninfo *screeninfo = &fb_data->spdc_fb_var;
+	u32 xres, yres;
+	int ret;
+
+	fb_data->fresh_param.buf_addr.next_buf_phys_addr =
+		fb_data->phys_start;
+
+	fb_data->hw_ready = true;
+	fb_data->hw_initializing = false;
+
+	/* Use unrotated (native) width/height */
+	if ((screeninfo->rotate == FB_ROTATE_CW) ||
+		(screeninfo->rotate == FB_ROTATE_CCW)) {
+		xres = screeninfo->yres;
+		yres = screeninfo->xres;
+	} else {
+		xres = screeninfo->xres;
+		yres = screeninfo->yres;
+	}
+
+	update.update_region.left = 0;
+	update.update_region.width = xres;
+	update.update_region.top = 0;
+	update.update_region.height = yres;
+	update.update_mode = UPDATE_MODE_FULL;
+	update.waveform_mode = fb_data->wv_modes.mode_init;
+	update.update_marker = INIT_UPDATE_MARKER;
+	update.temp = SPDC_DEFAULT_TEMP;
+	update.flags = 0;
+
+	upd_marker_data.update_marker = update.update_marker;
+
+	mxc_spdc_fb_send_update(&update, &fb_data->info);
+
+	/* Block on initial update */
+	ret = mxc_spdc_fb_wait_update_complete(&upd_marker_data,
+		&fb_data->info);
+	if (ret < 0)
+		dev_err(fb_data->dev,
+		"Wait for update complete failed, Err:%d", ret);
+}
+
+static void
+spdc_fb_fw_handler(const struct firmware *fw, void *context)
+{
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)context;
+	struct clk *spdc_parent;
+	unsigned long rounded_parent_rate, spdc_pix_rate,
+			rounded_pix_clk, target_pix_clk;
+	u8 *wv_file;
+	int ret;
+
+	if (fw == NULL) {
+		/* If default FW file load failed, we give up */
+		if (fb_data->fw_default_load)
+			return;
+
+		/* Try to load default waveform */
+		fb_data->fw_default_load = true;
+
+		ret = request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
+				fb_data->fw_str, fb_data->dev, GFP_KERNEL,
+				fb_data, spdc_fb_fw_handler);
+		if (ret) {
+			dev_err(fb_data->dev,
+			"Failed to load waveform image with err %d\n", ret);
+			return;
+		}
+	}
+
+	wv_file = (u8 *)fw->data;
+	memcpy(fb_data->virt_lut_buf, wv_file, fw->size);
+
+	check_waveform((u32 *)wv_file, (u32 *)fb_data->virt_lut_buf,
+					fw->size / 4);
+	release_firmware(fw);
+
+	/* Enable clocks to access SPDC regs */
+	clk_enable(fb_data->spdc_clk_axi);
+
+	target_pix_clk = fb_data->cur_mode->vmode->pixclock;
+	/* Enable pix clk for SPDC */
+	clk_enable(fb_data->spdc_clk_pix);
+	rounded_pix_clk = clk_round_rate(fb_data->spdc_clk_pix, target_pix_clk);
+
+	if (((rounded_pix_clk >= target_pix_clk + target_pix_clk/100) ||
+		(rounded_pix_clk <= target_pix_clk - target_pix_clk/100))) {
+		/* Can't get close enough without changing parent clk */
+		spdc_parent = clk_get_parent(fb_data->spdc_clk_pix);
+		rounded_parent_rate =
+			clk_round_rate(spdc_parent, target_pix_clk);
+
+		spdc_pix_rate = target_pix_clk;
+		while (spdc_pix_rate < rounded_parent_rate)
+			spdc_pix_rate *= 2;
+		clk_set_rate(spdc_parent, spdc_pix_rate);
+
+		rounded_pix_clk =
+			clk_round_rate(fb_data->spdc_clk_pix, target_pix_clk);
+		if (((rounded_pix_clk >= target_pix_clk + target_pix_clk/100) ||
+		(rounded_pix_clk <= target_pix_clk - target_pix_clk/100)))
+			/* Still can't get a good clock, provide warning */
+			dev_err(fb_data->dev,
+				"Unable to get an accurate SPDC pix clk"
+				"desired = %lu, actual = %lu\n", target_pix_clk,
+				rounded_pix_clk);
+	}
+
+	clk_set_rate(fb_data->spdc_clk_pix, rounded_pix_clk);
+
+	if (!spdc_init_sequence(fb_data))
+		return;
+
+	/* display log on picture */
+	spdc_draw_mode0(fb_data);
+}
+
+
+static int spdc_fb_init_hw(struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	int ret;
+
+	fb_data->fw_default_load = false;
+	/*
+	 * Create fw search string based on ID string in selected videomode.
+	 * Format is "imx/spdc_[wave_timing].fw: spdc_pvi.fw, spdc_auo.fw"
+	 */
+	if (fb_data->cur_mode) {
+		memset(fb_data->fw_str, 0, sizeof(fb_data->fw_str));
+		strcat(fb_data->fw_str, "imx/spdc_");
+		strcat(fb_data->fw_str, fb_data->cur_mode->wave_timing);
+		strcat(fb_data->fw_str, ".fw");
+	} else
+		strcat(fb_data->fw_str, "imx/spdc_pvi.fw");
+
+	ret = request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
+				fb_data->fw_str, fb_data->dev, GFP_KERNEL,
+				fb_data, spdc_fb_fw_handler);
+	if (ret) {
+		dev_err(fb_data->dev,
+			"Failed to load waveform image with err %d\n", ret);
+		return ret;
+	}
+
+	return ret;
+}
+
+static int mxc_spdc_partial_refresh(mxc_spdc_t *fb_data, void *buffer)
+{
+	int ret = 0;
+	struct partial_refresh_param *fresh_param = &fb_data->fresh_param;
+
+	if (!fb_data->panel_set.power_ready)
+		spdc_powerup(fb_data);
+
+	if (!fresh_param->blocking) {
+		if (mxc_spdc_device_is_busy(fb_data)) {
+			dev_err(fb_data->dev, "spdc busy!\n");
+			return (u32) -1;
+		}
+	} else {
+		while (mxc_spdc_device_is_busy(fb_data)) {
+			dev_err(fb_data->dev, "Waiting for spdc idle..\n");
+			msleep(500);
+		}
+	}
+
+	ret = mxc_spdc_partial_refresh_low(fb_data, buffer);
+
+	return ret;
+}
+
+static int mxc_operaton_update(mxc_spdc_t *fb_data)
+{
+	int ret = 0;
+	struct partial_refresh_param *fresh_param = &fb_data->fresh_param;
+	u32 operation_mode = fb_data->operation_mode;
+
+	if (!fb_data->panel_set.power_ready)
+		spdc_powerup(fb_data);
+
+	if (operation_mode != SPDC_SW_TCON_RESET) {
+		if (!fresh_param->blocking) {
+			if (mxc_spdc_device_is_busy(fb_data)) {
+				dev_err(fb_data->dev,  "spdc busy\n");
+				return (u32) -1;
+			}
+		} else {
+			while (mxc_spdc_device_is_busy(fb_data)) {
+				dev_err(fb_data->dev, "Waiting spdc idle...\n");
+				msleep(500);
+			}
+		}
+	} else
+		operation_mode = SPDC_SW_TCON_RESET_SET;
+
+	/* don't add to queue list */
+	mutex_lock(&fb_data->queue_mutex);
+	__raw_writel(operation_mode, fb_data->hwp + SPDC_OPERATE);
+	mutex_unlock(&fb_data->queue_mutex);
+
+	if (operation_mode == SPDC_SW_TCON_RESET_SET) {
+		dev_dbg(fb_data->dev, "reinit hw\n");
+		mdelay(500);
+
+		fb_data->hw_ready = false;
+		fb_data->operation_mode = SPDC_NO_OPERATION;
+		ret = spdc_fb_init_hw(&fb_data->info);
+		if (ret && !fb_data->hw_ready)
+			dev_err(fb_data->dev, "Failed to init HW!\n");
+	}
+
+	return ret;
+}
+
+static int mxc_spdc_refresh_display(mxc_spdc_t *fb_data)
+{
+	struct partial_refresh_param *fresh_param = &fb_data->fresh_param;
+	u32 operation_mode = fb_data->operation_mode;
+	int ret = 0;
+
+	fresh_param->update_region.left = 0;
+	fresh_param->update_region.top = 0;
+	fresh_param->update_region.width = fb_data->spdc_fb_var.xres;
+	fresh_param->update_region.height = fb_data->spdc_fb_var.yres;
+	fresh_param->stride = (fb_data->spdc_fb_var.xres *
+				fb_data->spdc_fb_var.bits_per_pixel) >> 3;
+
+	if (operation_mode && operation_mode < SPDC_FULL_REFRESH)
+		ret = mxc_operaton_update(fb_data);
+	else
+		ret = mxc_spdc_partial_refresh(fb_data, NULL);
+
+	return ret;
+}
+
+static void mxc_spdc_find_match_mode(mxc_spdc_t *fb_data)
+{
+	struct imx_spdc_fb_mode *spdc_mode =
+		&fb_data->pdata->spdc_mode[0];
+	const struct mxc_spdc_resolution_map_para *spdc_res_map;
+	u32 i = 0;
+	u32 j = 0;
+	u32 default_mode = 0xFF;
+
+	if (fb_data->panel_set.rgbw_mode_enable)
+		spdc_res_map = &spdc_rgbw_res_map[0];
+	else
+		spdc_res_map = &spdc_gray_res_map[0];
+
+	while (spdc_mode != NULL) {
+		while (spdc_res_map[j].resolution != 0xFFFF) {
+			if (spdc_mode->vmode->xres == spdc_res_map[j].res_x
+			&& spdc_mode->vmode->yres == spdc_res_map[j].res_y) {
+				fb_data->panel_set.resolution =
+					spdc_res_map[j].resolution;
+				default_mode = i;
+				break;
+			}
+			j++;
+		}
+
+		if (default_mode != 0xFF)
+			break;
+		j = 0;
+		i++;
+		spdc_mode = &fb_data->pdata->spdc_mode[i];
+	}
+
+	fb_data->cur_mode = spdc_mode;
+}
+
+static int mxc_spdc_fb_mmap(struct fb_info *info, struct vm_area_struct *vma)
+{
+	unsigned long start = vma->vm_start;
+	unsigned long size = vma->vm_end - vma->vm_start;
+	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
+	unsigned long page, pos;
+
+	if (offset + size > info->fix.smem_len)
+		return -EINVAL;
+
+	pos = (unsigned long)info->fix.smem_start + offset;
+
+	/* make buffers bufferable */
+	vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
+	vma->vm_flags |= VM_RESERVED | VM_IO;
+
+	while (size > 0) {
+		page = pos;
+		if (io_remap_pfn_range(vma, start, page >> PAGE_SHIFT,
+					PAGE_SIZE, vma->vm_page_prot))
+			return -EAGAIN;
+
+		start += PAGE_SIZE;
+		pos += PAGE_SIZE;
+		if (size > PAGE_SIZE)
+			size -= PAGE_SIZE;
+		else
+			size = 0;
+	}
+
+	return 0;
+}
+
+static inline u_int _chan_to_field(u_int chan, struct fb_bitfield *bf)
+{
+	chan &= 0xffff;
+	chan >>= 16 - bf->length;
+	return chan << bf->offset;
+}
+
+static int mxc_spdc_fb_setcolreg(u_int regno, u_int red, u_int green,
+				u_int blue, u_int transp, struct fb_info *info)
+{
+	if (regno >= 256) /* no. of hw registers */
+		return 1;
+
+	/* grayscale works only partially under directcolor */
+	if (info->var.grayscale) {
+		/* grayscale = 0.30*R + 0.59*G + 0.11*B */
+		red = green = blue = (red * 77 + green * 151 + blue * 28) >> 8;
+	}
+
+#define CNVT_TOHW(val, width) ((((val)<<(width))+0x7FFF-(val))>>16)
+	switch (info->fix.visual) {
+	case FB_VISUAL_TRUECOLOR:
+	case FB_VISUAL_PSEUDOCOLOR:
+		red = CNVT_TOHW(red, info->var.red.length);
+		green = CNVT_TOHW(green, info->var.green.length);
+		blue = CNVT_TOHW(blue, info->var.blue.length);
+		transp = CNVT_TOHW(transp, info->var.transp.length);
+		break;
+	case FB_VISUAL_DIRECTCOLOR:
+		red = CNVT_TOHW(red, 8); /* expect 8 bit DAC */
+		green = CNVT_TOHW(green, 8);
+		blue = CNVT_TOHW(blue, 8);
+		/* hey, there is bug in transp handling... */
+		transp = CNVT_TOHW(transp, 8);
+		break;
+	}
+#undef CNVT_TOHW
+	/* Truecolor has hardware independent palette */
+	if (info->fix.visual == FB_VISUAL_TRUECOLOR) {
+		if (regno >= 16)
+			return 1;
+
+		((u32 *) (info->pseudo_palette))[regno] =
+		(red << info->var.red.offset) |
+		(green << info->var.green.offset) |
+		(blue << info->var.blue.offset) |
+		(transp << info->var.transp.offset);
+	}
+
+	return 0;
+}
+
+void mxc_spdc_fb_flush_updates(mxc_spdc_t *fb_data)
+{
+	int ret;
+
+	/* Grab queue lock to prevent any new updates from being submitted */
+	mutex_lock(&fb_data->queue_mutex);
+
+	/*
+	 * 3 places to check for updates that are active or pending:
+	 *   1) Updates in the pending list
+	 *   2) Update buffers in use (e.g., PxP processing)
+	 *   3) Active updates to panel - We can key off of SPDC
+	 *      power state to know if we have active updates.
+	 */
+	if (!list_empty(&fb_data->upd_pending_list) ||
+		!is_free_list_full(fb_data) ||
+		(fb_data->updates_active == true)) {
+		/* Initialize event signalling updates are done */
+		init_completion(&fb_data->updates_done);
+		fb_data->waiting_for_idle = true;
+
+		mutex_unlock(&fb_data->queue_mutex);
+		/* Wait for any currently active updates to complete */
+		ret = wait_for_completion_timeout(&fb_data->updates_done,
+						msecs_to_jiffies(8000));
+		if (!ret)
+			dev_err(fb_data->dev,
+				"Flush updates timeout! ret = 0x%x\n", ret);
+
+		mutex_lock(&fb_data->queue_mutex);
+		fb_data->waiting_for_idle = false;
+	}
+
+	mutex_unlock(&fb_data->queue_mutex);
+}
+
+static int mxc_spdc_fb_setcmap(struct fb_cmap *cmap, struct fb_info *info)
+{
+	int count, index, r;
+	u16 *red, *green, *blue, *transp;
+	u16 trans = 0xffff;
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	int i;
+
+	dev_dbg(fb_data->dev, "setcmap\n");
+
+	if (info->fix.visual == FB_VISUAL_STATIC_PSEUDOCOLOR) {
+		/* Only support an 8-bit, 256 entry lookup */
+		if (cmap->len != 256)
+			return 1;
+
+		mxc_spdc_fb_flush_updates(fb_data);
+
+		mutex_lock(&fb_data->pxp_mutex);
+		/*
+		 * Store colormap in pxp_conf structure for later transmit
+		 * to PxP during update process to convert gray pixels.
+		 *
+		 * Since red=blue=green for pseudocolor visuals, we can
+		 * just use red values.
+		 */
+		for (i = 0; i < 256; i++)
+			fb_data->pxp_conf.proc_data.lut_map[i] =
+					cmap->red[i] & 0xFF;
+
+		fb_data->pxp_conf.proc_data.lut_map_updated = true;
+
+		mutex_unlock(&fb_data->pxp_mutex);
+	} else {
+		red = cmap->red;
+		green   = cmap->green;
+		blue    = cmap->blue;
+		transp  = cmap->transp;
+		index   = cmap->start;
+
+		for (count = 0; count < cmap->len; count++) {
+			if (transp)
+				trans = *transp++;
+			r = mxc_spdc_fb_setcolreg(index++, *red++,
+				*green++, *blue++, trans, info);
+			if (r != 0)
+				return r;
+		}
+	}
+
+	return 0;
+}
+
+static void adjust_coordinates(u32 xres, u32 yres, u32 rotation,
+	struct mxcfb_rect *update_region, struct mxcfb_rect *adj_update_region)
+{
+	u32 temp;
+
+	/* If adj_update_region == NULL, pass result back in update_region */
+	/* If adj_update_region == valid, use it to pass back result */
+	if (adj_update_region)
+		switch (rotation) {
+		case FB_ROTATE_UR:
+			adj_update_region->top = update_region->top;
+			adj_update_region->left = update_region->left;
+			adj_update_region->width = update_region->width;
+			adj_update_region->height = update_region->height;
+			break;
+		case FB_ROTATE_CW:
+			adj_update_region->top = update_region->left;
+			adj_update_region->left = yres -
+				(update_region->top + update_region->height);
+			adj_update_region->width = update_region->height;
+			adj_update_region->height = update_region->width;
+			break;
+		case FB_ROTATE_UD:
+			adj_update_region->width = update_region->width;
+			adj_update_region->height = update_region->height;
+			adj_update_region->top = yres -
+				(update_region->top + update_region->height);
+			adj_update_region->left = xres -
+				(update_region->left + update_region->width);
+			break;
+		case FB_ROTATE_CCW:
+			adj_update_region->left = update_region->top;
+			adj_update_region->top = xres -
+				(update_region->left + update_region->width);
+			adj_update_region->width = update_region->height;
+			adj_update_region->height = update_region->width;
+			break;
+		}
+	else
+		switch (rotation) {
+		case FB_ROTATE_UR:
+			/* No adjustment needed */
+			break;
+		case FB_ROTATE_CW:
+			temp = update_region->top;
+			update_region->top = update_region->left;
+			update_region->left = yres -
+				(temp + update_region->height);
+			temp = update_region->width;
+			update_region->width = update_region->height;
+			update_region->height = temp;
+			break;
+		case FB_ROTATE_UD:
+			update_region->top = yres -
+				(update_region->top + update_region->height);
+			update_region->left = xres -
+				(update_region->left + update_region->width);
+			break;
+		case FB_ROTATE_CCW:
+			temp = update_region->left;
+			update_region->left = update_region->top;
+			update_region->top = xres -
+				(temp + update_region->width);
+			temp = update_region->width;
+			update_region->width = update_region->height;
+			update_region->height = temp;
+			break;
+		}
+}
+
+static int mxc_spdc_fb_set_fix(struct fb_info *info)
+{
+	struct fb_fix_screeninfo *fix = &info->fix;
+	struct fb_var_screeninfo *var = &info->var;
+
+	fix->line_length = var->xres_virtual * var->bits_per_pixel / 8;
+
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->accel = FB_ACCEL_NONE;
+	if (var->grayscale)
+		fix->visual = FB_VISUAL_STATIC_PSEUDOCOLOR;
+	else
+		fix->visual = FB_VISUAL_TRUECOLOR;
+		fix->xpanstep = 1;
+	fix->ypanstep = 1;
+
+	return 0;
+}
+
+static int mxc_spdc_fb_set_par(struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	struct fb_var_screeninfo *screeninfo = &fb_data->info.var;
+	struct imx_spdc_fb_mode *spdc_modes = fb_data->pdata->spdc_mode;
+	struct pxp_config_data *pxp_conf = &fb_data->pxp_conf;
+	struct pxp_proc_data *proc_data = &pxp_conf->proc_data;
+	int i, ret;
+	__u32 xoffset_old, yoffset_old;
+
+	/*
+	 * Can't change the FB parameters until current updates have completed.
+	 * This function returns when all active updates are done.
+	 */
+	mxc_spdc_fb_flush_updates(fb_data);
+
+	mutex_lock(&fb_data->queue_mutex);
+	/*
+	 * Set all screeninfo except for xoffset/yoffset
+	 * Subsequent call to pan_display will handle those.
+	 */
+	xoffset_old = fb_data->spdc_fb_var.xoffset;
+	yoffset_old = fb_data->spdc_fb_var.yoffset;
+	fb_data->spdc_fb_var = *screeninfo;
+	fb_data->spdc_fb_var.xoffset = xoffset_old;
+	fb_data->spdc_fb_var.yoffset = yoffset_old;
+	mutex_unlock(&fb_data->queue_mutex);
+
+	mutex_lock(&fb_data->pxp_mutex);
+
+	/*
+	 * Update PxP config data (used to process FB regions for updates)
+	 * based on FB info and processing tasks required
+	 */
+	/* Initialize non-channel-specific PxP parameters */
+	proc_data->drect.left = proc_data->srect.left = 0;
+	proc_data->drect.top = proc_data->srect.top = 0;
+	proc_data->drect.width = proc_data->srect.width = screeninfo->xres;
+	proc_data->drect.height = proc_data->srect.height = screeninfo->yres;
+	proc_data->scaling = 0;
+	proc_data->hflip = 0;
+	proc_data->vflip = 0;
+	proc_data->rotate = screeninfo->rotate;
+	proc_data->bgcolor = 0;
+	proc_data->overlay_state = 0;
+	proc_data->lut_transform = PXP_LUT_NONE;
+
+	/*
+	 * configure S0 channel parameters
+	 * Parameters should match FB format/width/height
+	 */
+	if (screeninfo->grayscale)
+		pxp_conf->s0_param.pixel_fmt = PXP_PIX_FMT_GY04;
+	else {
+		switch (screeninfo->bits_per_pixel) {
+		case 16:
+			pxp_conf->s0_param.pixel_fmt = PXP_PIX_FMT_RGB565;
+			break;
+		case 24:
+			pxp_conf->s0_param.pixel_fmt = PXP_PIX_FMT_RGB24;
+			break;
+		case 32:
+			pxp_conf->s0_param.pixel_fmt = PXP_PIX_FMT_RGB32;
+			break;
+		default:
+			pxp_conf->s0_param.pixel_fmt = PXP_PIX_FMT_RGB565;
+			break;
+		}
+	}
+	pxp_conf->s0_param.width = screeninfo->xres_virtual;
+	pxp_conf->s0_param.height = screeninfo->yres;
+	pxp_conf->s0_param.color_key = -1;
+	pxp_conf->s0_param.color_key_enable = false;
+
+	/*
+	 * Initialize Output channel parameters
+	 * Output is Y-only greyscale
+	 * Output width/height will vary based on update region size
+	 */
+	pxp_conf->out_param.width = screeninfo->xres;
+	pxp_conf->out_param.height = screeninfo->yres;
+	pxp_conf->out_param.pixel_fmt = PXP_PIX_FMT_GY04;
+
+	mutex_unlock(&fb_data->pxp_mutex);
+
+	/* active new config, If HW not yet initialized,
+	 * check to see if we are being sent
+	 * an initialization request.
+	 */
+	if (!fb_data->hw_ready) {
+		struct fb_videomode mode;
+		bool found_match = false;
+		u32 xres_temp;
+
+		fb_var_to_videomode(&mode, screeninfo);
+
+		/* When comparing requested fb mode,
+		 * we need to use unrotated dimensions
+		 */
+		if ((screeninfo->rotate == FB_ROTATE_CW) ||
+			(screeninfo->rotate == FB_ROTATE_CCW)) {
+			xres_temp = mode.xres;
+			mode.xres = mode.yres;
+			mode.yres = xres_temp;
+		}
+
+		/* Match videomode against spdc modes */
+		for (i = 0; i < fb_data->pdata->num_modes; i++) {
+			if (!fb_mode_is_equal(spdc_modes[i].vmode, &mode))
+				continue;
+			fb_data->cur_mode = &spdc_modes[i];
+			found_match = true;
+			break;
+		}
+
+		if (!found_match) {
+			dev_err(fb_data->dev,
+				"Failed to match requested video mode\n");
+			return EINVAL;
+		}
+
+		/* Initialize SPDC settings and init panel */
+		ret =
+		spdc_fb_init_hw((struct fb_info *)fb_data);
+		if (ret) {
+			dev_err(fb_data->dev,
+				"Failed to load panel waveform data\n");
+			return ret;
+		}
+	}
+
+	mxc_spdc_fb_set_fix(info);
+
+	return 0;
+}
+
+static int
+mxc_spdc_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+
+	if (!var->xres)
+		var->xres = 1;
+	if (!var->yres)
+		var->yres = 1;
+
+	if (var->xres_virtual < var->xoffset + var->xres)
+		var->xres_virtual = var->xoffset + var->xres;
+	if (var->yres_virtual < var->yoffset + var->yres)
+		var->yres_virtual = var->yoffset + 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 != 4))
+		var->bits_per_pixel = SPDC_DEFAULT_BPP;
+
+	switch (var->bits_per_pixel) {
+	case 4:
+		var->red.offset = 0;
+		var->red.length = var->bits_per_pixel;
+		var->green = var->red;
+		var->blue = var->red;
+		var->transp.offset = 0;
+		var->transp.length = 0;
+		break;
+	case 8:
+		if (var->grayscale != 0) {
+			var->red.length = 8;
+			var->red.offset = 0;
+			var->red.msb_right = 0;
+
+			var->green = var->red;
+			var->blue = var->red;
+			var->transp.length = 0;
+			var->transp.offset = 0;
+			var->transp.msb_right = 0;
+		} else {
+			var->red.length = 3;
+			var->red.offset = 5;
+			var->red.msb_right = 0;
+
+			var->green.length = 3;
+			var->green.offset = 2;
+			var->green.msb_right = 0;
+
+			var->blue.length = 2;
+			var->blue.offset = 0;
+			var->blue.msb_right = 0;
+
+			var->transp.length = 0;
+			var->transp.offset = 0;
+			var->transp.msb_right = 0;
+		}
+		break;
+	case 16:
+		var->red.length = 5;
+		var->red.offset = 11;
+		var->red.msb_right = 0;
+
+		var->green.length = 6;
+		var->green.offset = 5;
+		var->green.msb_right = 0;
+
+		var->blue.length = 5;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 24:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 0;
+		var->transp.offset = 0;
+		var->transp.msb_right = 0;
+		break;
+	case 32:
+		var->red.length = 8;
+		var->red.offset = 16;
+		var->red.msb_right = 0;
+
+		var->green.length = 8;
+		var->green.offset = 8;
+		var->green.msb_right = 0;
+
+		var->blue.length = 8;
+		var->blue.offset = 0;
+		var->blue.msb_right = 0;
+
+		var->transp.length = 8;
+		var->transp.offset = 24;
+		var->transp.msb_right = 0;
+		break;
+	}
+
+	switch (var->rotate) {
+	case FB_ROTATE_UR:
+	case FB_ROTATE_UD:
+		var->xres = fb_data->native_width;
+		var->yres = fb_data->native_height;
+		break;
+	case FB_ROTATE_CW:
+	case FB_ROTATE_CCW:
+		var->xres = fb_data->native_height;
+		var->yres = fb_data->native_width;
+		break;
+	default:
+		/* Invalid rotation value */
+		var->rotate = 0;
+		dev_dbg(fb_data->dev, "Invalid rotation request\n");
+		return -EINVAL;
+	}
+
+	var->xres_virtual = ALIGN(var->xres, 32);
+	var->yres_virtual = ALIGN(var->yres, 128) * fb_data->num_screens;
+
+	var->height = -1;
+	var->width = -1;
+
+	return 0;
+}
+
+void mxc_spdc_fb_set_waveform_modes(struct mxcfb_waveform_modes *modes,
+	struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = info ?
+		(mxc_spdc_t *)info:g_fb_data;
+
+	mutex_lock(&fb_data->queue_mutex);
+
+	memcpy(&fb_data->wv_modes, modes, sizeof(struct mxcfb_waveform_modes));
+
+	mutex_unlock(&fb_data->queue_mutex);
+}
+EXPORT_SYMBOL(mxc_spdc_fb_set_waveform_modes);
+
+/* To stick with non-fractional degrees for the sake
+ *  of API consistency with EPDC.
+ */
+int mxc_spdc_fb_set_temperature(int temperature, struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = info ?
+		(mxc_spdc_t *)info:g_fb_data;
+	s8 temper = (s8)(temperature & 0xFF) << 1;
+
+	mutex_lock(&fb_data->queue_mutex);
+
+	if (temper > -110 && temper < 200)
+		__raw_writel(temper, fb_data->hwp + SPDC_TEMP_INFO);
+	else
+		__raw_writel(SPDC_DEFAULT_TEMP, fb_data->hwp + SPDC_TEMP_INFO);
+
+	mutex_unlock(&fb_data->queue_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL(mxc_spdc_fb_set_temperature);
+
+
+int mxc_spdc_fb_set_auto_update(u32 auto_mode, struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = info ?
+		(mxc_spdc_t *)info:g_fb_data;
+
+	dev_dbg(fb_data->dev, "Setting auto update mode to %d\n", auto_mode);
+
+	if ((auto_mode == AUTO_UPDATE_MODE_AUTOMATIC_MODE)
+		|| (auto_mode == AUTO_UPDATE_MODE_REGION_MODE))
+		fb_data->auto_mode = auto_mode;
+	else {
+		dev_err(fb_data->dev, "Invalid auto update mode parameter.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(mxc_spdc_fb_set_auto_update);
+
+
+int mxc_spdc_fb_set_upd_scheme(u32 upd_scheme, struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = info ?
+		(mxc_spdc_t *)info:g_fb_data;
+
+	dev_dbg(fb_data->dev, "Setting optimization level to %d\n", upd_scheme);
+
+	/*
+	 * Can't change the scheme until current updates have completed.
+	 * This function returns when all active updates are done.
+	 */
+	mxc_spdc_fb_flush_updates(fb_data);
+
+	if ((upd_scheme == UPDATE_SCHEME_SNAPSHOT)
+		|| (upd_scheme == UPDATE_SCHEME_QUEUE)
+		|| (upd_scheme == UPDATE_SCHEME_QUEUE_AND_MERGE))
+		fb_data->upd_scheme = upd_scheme;
+	else {
+		dev_err(fb_data->dev, "Invalid update scheme specified.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(mxc_spdc_fb_set_upd_scheme);
+
+/* Callback function triggered after PxP receives an EOF interrupt */
+static void pxp_dma_done(void *arg)
+{
+	struct pxp_tx_desc *tx_desc = to_tx_desc(arg);
+	struct dma_chan *chan = tx_desc->txd.chan;
+	struct pxp_channel *pxp_chan = to_pxp_channel(chan);
+	mxc_spdc_t *fb_data = pxp_chan->client;
+
+	/* This call will signal wait_for_completion_timeout()
+	 * in send_buffer_to_pxp
+	 */
+	complete(&fb_data->pxp_tx_cmpl);
+}
+
+static bool chan_filter(struct dma_chan *chan, void *arg)
+{
+	if (imx_dma_is_pxp(chan))
+		return true;
+	else
+		return false;
+}
+
+/* Function to request PXP DMA channel */
+static int pxp_chan_init(mxc_spdc_t *fb_data)
+{
+	dma_cap_mask_t mask;
+	struct dma_chan *chan;
+
+	/*
+	 * Request a free channel
+	 */
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+	dma_cap_set(DMA_PRIVATE, mask);
+	chan = dma_request_channel(mask, chan_filter, NULL);
+	if (!chan) {
+		dev_err(fb_data->dev, "Unsuccessfully received channel!!!!\n");
+		return -EBUSY;
+	}
+
+	fb_data->pxp_chan = to_pxp_channel(chan);
+	fb_data->pxp_chan->client = fb_data;
+
+	init_completion(&fb_data->pxp_tx_cmpl);
+
+	return 0;
+}
+
+/*
+ * Function to call PxP DMA driver and send our latest FB update region
+ * through the PxP and out to an intermediate buffer.
+ * Note: This is a blocking call, so upon return the PxP tx should be complete.
+ */
+static int pxp_process_update(mxc_spdc_t *fb_data,
+			  u32 src_width, u32 src_height,
+			  struct mxcfb_rect *update_region)
+{
+	dma_cookie_t cookie;
+	struct scatterlist *sg = fb_data->sg;
+	struct dma_chan *dma_chan;
+	struct pxp_tx_desc *desc;
+	struct dma_async_tx_descriptor *txd;
+	struct pxp_config_data *pxp_conf = &fb_data->pxp_conf;
+	struct pxp_proc_data *proc_data = &fb_data->pxp_conf.proc_data;
+	int i, ret;
+	int length;
+
+	/* First, check to see that we have acquired a PxP Channel object */
+	if (fb_data->pxp_chan == NULL) {
+		/*
+		 * PxP Channel has not yet been created and initialized,
+		 * so let's go ahead and try
+		 */
+		ret = pxp_chan_init(fb_data);
+		if (ret) {
+			/*
+			 * PxP channel init failed, and we can't use the
+			 * PxP until the PxP DMA driver has loaded, so we abort
+			 */
+			dev_err(fb_data->dev, "PxP chan init failed\n");
+			return -ENODEV;
+		}
+	}
+
+	/*
+	 * Init completion, so that we
+	 * can be properly informed of the completion
+	 * of the PxP task when it is done.
+	 */
+	init_completion(&fb_data->pxp_tx_cmpl);
+
+	dma_chan = &fb_data->pxp_chan->dma_chan;
+
+	txd = dma_chan->device->device_prep_slave_sg(dma_chan, sg, 2,
+						 DMA_TO_DEVICE,
+						 DMA_PREP_INTERRUPT);
+	if (!txd) {
+		dev_err(fb_data->info.device,
+			"Error preparing a DMA transaction descriptor.\n");
+		return -EIO;
+	}
+
+	txd->callback_param = txd;
+	txd->callback = pxp_dma_done;
+
+	/*
+	 * Configure PxP for processing of new update region
+	 * The rest of our config params were set up in
+	 * probe() and should not need to be changed.
+	 */
+	pxp_conf->s0_param.width = src_width;
+	pxp_conf->s0_param.height = src_height;
+	proc_data->srect.top = update_region->top;
+	proc_data->srect.left = update_region->left;
+	proc_data->srect.width = update_region->width;
+	proc_data->srect.height = update_region->height;
+
+	/*
+	 * Because only YUV/YCbCr image can be scaled, configure
+	 * drect equivalent to srect, as such do not perform scaling.
+	 */
+	proc_data->drect.top = 0;
+	proc_data->drect.left = 0;
+	proc_data->drect.width = proc_data->srect.width;
+	proc_data->drect.height = proc_data->srect.height;
+
+	/* PXP expects rotation in terms of degrees */
+	proc_data->rotate = fb_data->spdc_fb_var.rotate * 90;
+	if (proc_data->rotate > 270)
+		proc_data->rotate = 0;
+
+	pxp_conf->out_param.width = update_region->width;
+	pxp_conf->out_param.height = update_region->height;
+
+	if ((proc_data->rotate == 90) || (proc_data->rotate == 270))
+		pxp_conf->out_param.stride = update_region->height;
+	else
+		pxp_conf->out_param.stride = update_region->width;
+
+	desc = to_tx_desc(txd);
+	length = desc->len;
+	for (i = 0; i < length; i++) {
+		if (i == 0) {/* S0 */
+			memcpy(&desc->proc_data, proc_data,
+				sizeof(struct pxp_proc_data));
+			pxp_conf->s0_param.paddr = sg_dma_address(&sg[0]);
+			memcpy(&desc->layer_param.s0_param, &pxp_conf->s0_param,
+				sizeof(struct pxp_layer_param));
+		} else if (i == 1) {
+			pxp_conf->out_param.paddr = sg_dma_address(&sg[1]);
+			memcpy(&desc->layer_param.out_param,
+					&pxp_conf->out_param,
+					sizeof(struct pxp_layer_param));
+		}
+		/* TODO: OverLay */
+
+		desc = desc->next;
+	}
+
+	/* Submitting our TX starts the PxP processing task */
+	cookie = txd->tx_submit(txd);
+	if (cookie < 0) {
+		dev_err(fb_data->info.device, "Error sending FB through PxP\n");
+		return -EIO;
+	}
+
+	fb_data->txd = txd;
+
+	/* trigger ePxP */
+	dma_async_issue_pending(dma_chan);
+
+	return 0;
+}
+
+static int pxp_complete_update(mxc_spdc_t *fb_data, u32 *hist_stat)
+{
+	int ret;
+	/*
+	 * Wait for completion event, which will be set
+	 * through our TX callback function.
+	 */
+	ret = wait_for_completion_timeout(&fb_data->pxp_tx_cmpl, HZ / 10);
+	if (ret <= 0) {
+		dev_info(fb_data->info.device,
+			 "PxP operation failed due to %s\n",
+			 ret < 0 ? "user interrupt" : "timeout");
+		dma_release_channel(&fb_data->pxp_chan->dma_chan);
+		fb_data->pxp_chan = NULL;
+		return ret ? : -ETIMEDOUT;
+	}
+
+	if ((fb_data->pxp_conf.proc_data.lut_transform & EPDC_FLAG_USE_CMAP) &&
+		fb_data->pxp_conf.proc_data.lut_map_updated)
+		fb_data->pxp_conf.proc_data.lut_map_updated = false;
+
+	*hist_stat = to_tx_desc(fb_data->txd)->hist_status;
+	dma_release_channel(&fb_data->pxp_chan->dma_chan);
+	fb_data->pxp_chan = NULL;
+
+	dev_dbg(fb_data->dev, "TX completed\n");
+
+	return 0;
+}
+
+static void copy_to_next_buffer(mxc_spdc_t *fb_data,
+	struct update_data_list *upd_data_list)
+{
+	struct mxcfb_update_data *upd_data =
+		&upd_data_list->update_desc->upd_data;
+	unsigned char *temp_buf_ptr = fb_data->virt_addr_copybuf;
+	unsigned char *dst_ptr = upd_data_list->virt_addr;
+	struct mxcfb_rect adj_update_region;
+	int dst_stride, left_offs, line_width;
+	int i;
+
+	switch (fb_data->spdc_fb_var.rotate) {
+	case FB_ROTATE_UR:
+		adj_update_region.top = upd_data->update_region.top;
+		adj_update_region.left = upd_data->update_region.left;
+		adj_update_region.width = upd_data->update_region.width;
+		adj_update_region.height = upd_data->update_region.height;
+		dst_stride = fb_data->spdc_fb_var.xres_virtual / 2;
+		break;
+	case FB_ROTATE_CW:
+		adj_update_region.top = upd_data->update_region.left;
+		adj_update_region.left = fb_data->spdc_fb_var.yres -
+				(upd_data->update_region.top +
+				upd_data->update_region.height);
+		adj_update_region.width = upd_data->update_region.height;
+		adj_update_region.height = upd_data->update_region.width;
+		dst_stride = fb_data->spdc_fb_var.yres / 2;
+		break;
+	case FB_ROTATE_UD:
+		adj_update_region.width = upd_data->update_region.width;
+		adj_update_region.height = upd_data->update_region.height;
+		adj_update_region.top = fb_data->spdc_fb_var.yres -
+		(upd_data->update_region.top + upd_data->update_region.height);
+		adj_update_region.left = fb_data->spdc_fb_var.xres -
+				(upd_data->update_region.left +
+				upd_data->update_region.width);
+		dst_stride = fb_data->spdc_fb_var.xres_virtual / 2;
+		break;
+	case FB_ROTATE_CCW:
+		adj_update_region.left = upd_data->update_region.top;
+		adj_update_region.top = fb_data->spdc_fb_var.xres -
+				(upd_data->update_region.left +
+				upd_data->update_region.width);
+		adj_update_region.width = upd_data->update_region.height;
+		adj_update_region.height = upd_data->update_region.width;
+		dst_stride = fb_data->spdc_fb_var.yres / 2;
+		break;
+	}
+
+	/* pxp output Y4 data.
+	 * Copy the raw data to related region in next buffer.
+	 */
+	left_offs = adj_update_region.left / 2;
+	line_width = adj_update_region.width / 2;
+
+	dst_ptr += (adj_update_region.top * dst_stride + left_offs);
+	for (i = 0; i < adj_update_region.height; i++) {
+		/* Copy the full line */
+		memcpy(dst_ptr,	temp_buf_ptr, line_width);
+
+		dst_ptr += dst_stride;
+		temp_buf_ptr += line_width;
+	}
+}
+
+static int spdc_process_update(struct update_data_list *upd_data_list,
+				   mxc_spdc_t *fb_data)
+{
+	/* Region of src buffer for update */
+	struct mxcfb_rect *src_upd_region;
+	struct mxcfb_rect pxp_upd_region;
+	struct update_desc_list *upd_desc_list = upd_data_list->update_desc;
+	u32 src_width, src_height;
+	u32 offset_from_4, bytes_per_pixel;
+	u32 post_rotation_xcoord, post_rotation_ycoord, width_pxp_blocks;
+	u32 pxp_input_offs, pxp_output_offs, pxp_output_shift;
+	bool input_unaligned = false;
+	u32 hist_stat = 0;
+	bool use_temp_buf = false;
+	int ret;
+
+	/*
+	 * Are we using FB or an alternate (overlay)
+	 * buffer for source of update?
+	 */
+	if (upd_desc_list->upd_data.flags & EPDC_FLAG_USE_ALT_BUFFER) {
+		src_width = upd_desc_list->upd_data.alt_buffer_data.width;
+		src_height = upd_desc_list->upd_data.alt_buffer_data.height;
+		src_upd_region =
+		&upd_desc_list->upd_data.alt_buffer_data.alt_update_region;
+	} else {
+		src_width = fb_data->spdc_fb_var.xres_virtual;
+		src_height = fb_data->spdc_fb_var.yres;
+		src_upd_region = &upd_desc_list->upd_data.update_region;
+	}
+
+	if (!(src_upd_region->width == fb_data->spdc_fb_var.xres_virtual &&
+		fb_data->spdc_fb_var.rotate == FB_ROTATE_UR))
+		use_temp_buf = true;
+
+	bytes_per_pixel = fb_data->spdc_fb_var.bits_per_pixel / 8;
+
+	/* Grab pxp_mutex here so that we protect access
+	 * to copybuf in addition to the PxP structures */
+	mutex_lock(&fb_data->pxp_mutex);
+
+	offset_from_4 = src_upd_region->left & 0x3;
+	input_unaligned = ((offset_from_4 * bytes_per_pixel % 4) != 0) ?
+				true : false;
+
+	if (input_unaligned) {
+		/* Leave a gap between PxP input addr
+		 * and update region pixels
+		 */
+		pxp_input_offs =
+			(src_upd_region->top * src_width + src_upd_region->left)
+			* bytes_per_pixel & 0xFFFFFFFC;
+		/* Update region left changes to reflect
+		 * relative position to input ptr
+		 */
+		pxp_upd_region.left = (offset_from_4 * bytes_per_pixel % 4)
+					/ bytes_per_pixel;
+	} else {
+		pxp_input_offs =
+			(src_upd_region->top * src_width + src_upd_region->left)
+			* bytes_per_pixel;
+		pxp_upd_region.left = 0;
+	}
+	pxp_upd_region.top = 0;
+
+	/* Update region dimensions to meet 8x8 pixel requirement */
+	if (fb_data->spdc_fb_var.rotate == 0) {
+		pxp_upd_region.width = ALIGN(src_upd_region->width, 8);
+		pxp_upd_region.height = ALIGN(src_upd_region->height, 8);
+	} else {
+		pxp_upd_region.width =
+			ALIGN(src_upd_region->width + pxp_upd_region.left, 8);
+		pxp_upd_region.height = ALIGN(src_upd_region->height, 8);
+	}
+
+	switch (fb_data->spdc_fb_var.rotate) {
+	case FB_ROTATE_UR:
+	default:
+		post_rotation_xcoord = pxp_upd_region.left;
+		post_rotation_ycoord = pxp_upd_region.top;
+		width_pxp_blocks = pxp_upd_region.width;
+		break;
+	case FB_ROTATE_CW:
+		width_pxp_blocks = pxp_upd_region.height;
+		post_rotation_xcoord = width_pxp_blocks -
+					src_upd_region->height;
+		post_rotation_ycoord = pxp_upd_region.left;
+		break;
+	case FB_ROTATE_UD:
+		width_pxp_blocks = pxp_upd_region.width;
+		post_rotation_xcoord = width_pxp_blocks -
+			src_upd_region->width -	pxp_upd_region.left;
+		post_rotation_ycoord = pxp_upd_region.height -
+			src_upd_region->height - pxp_upd_region.top;
+		break;
+	case FB_ROTATE_CCW:
+		width_pxp_blocks = pxp_upd_region.height;
+		post_rotation_xcoord = pxp_upd_region.top;
+		post_rotation_ycoord = pxp_upd_region.width -
+			src_upd_region->width -	pxp_upd_region.left;
+		break;
+	}
+
+	/* Update region start coord to force PxP to
+	 * process full 8x8 regions
+	 */
+	pxp_upd_region.top &= ~0x7;
+	pxp_upd_region.left &= ~0x7;
+
+	pxp_output_shift = ALIGN(post_rotation_xcoord, 8)
+		- post_rotation_xcoord;
+	pxp_output_offs = post_rotation_ycoord * width_pxp_blocks
+		+ pxp_output_shift;
+	upd_desc_list->spdc_offs = ALIGN(pxp_output_offs, 8);
+
+	/* Source address either comes from alternate buffer
+	   provided in update data, or from the framebuffer. */
+	if (upd_desc_list->upd_data.flags & EPDC_FLAG_USE_ALT_BUFFER)
+		sg_dma_address(&fb_data->sg[0]) =
+			upd_desc_list->upd_data.alt_buffer_data.phys_addr
+				+ pxp_input_offs;
+	else {
+		sg_dma_address(&fb_data->sg[0]) =
+			fb_data->info.fix.smem_start + fb_data->fb_offset
+			+ pxp_input_offs;
+		sg_set_page(&fb_data->sg[0],
+			virt_to_page(fb_data->info.screen_base),
+			fb_data->info.fix.smem_len,
+			offset_in_page(fb_data->info.screen_base));
+	}
+
+	/* Update sg[1] to point to output of PxP proc task */
+	if (!use_temp_buf) {
+		sg_dma_address(&fb_data->sg[1]) = upd_data_list->phys_addr;
+		sg_set_page(&fb_data->sg[1],
+				virt_to_page(upd_data_list->virt_addr),
+		fb_data->max_pix_size,
+		offset_in_page(upd_data_list->virt_addr));
+	} else {
+		sg_dma_address(&fb_data->sg[1]) = fb_data->phys_addr_copybuf;
+		sg_set_page(&fb_data->sg[1],
+			virt_to_page(fb_data->virt_addr_copybuf),
+		fb_data->max_pix_size,
+		offset_in_page(fb_data->virt_addr_copybuf));
+	}
+
+	/*
+	 * Set PxP LUT transform type based on update flags.
+	 */
+	fb_data->pxp_conf.proc_data.lut_transform = 0;
+	if (upd_desc_list->upd_data.flags & EPDC_FLAG_ENABLE_INVERSION)
+		fb_data->pxp_conf.proc_data.lut_transform |= PXP_LUT_INVERT;
+	if (upd_desc_list->upd_data.flags & EPDC_FLAG_FORCE_MONOCHROME)
+		fb_data->pxp_conf.proc_data.lut_transform |=
+			PXP_LUT_BLACK_WHITE;
+	if (upd_desc_list->upd_data.flags & EPDC_FLAG_USE_CMAP)
+		fb_data->pxp_conf.proc_data.lut_transform |=
+			PXP_LUT_USE_CMAP;
+
+	/*
+	 * Toggle inversion processing if 8-bit
+	 * inverted is the current pixel format.
+	 */
+	if (fb_data->spdc_fb_var.grayscale == GRAYSCALE_4BIT_INVERTED)
+		fb_data->pxp_conf.proc_data.lut_transform ^= PXP_LUT_INVERT;
+
+	/* This is a blocking call, so upon return PxP tx should be done */
+	ret = pxp_process_update(fb_data, src_width, src_height,
+		&pxp_upd_region);
+	if (ret) {
+		dev_err(fb_data->dev, "Unable to submit PxP update task.\n");
+		mutex_unlock(&fb_data->pxp_mutex);
+		return ret;
+	}
+
+	/* If needed, enable SPDC HW while ePxP is processing */
+	if ((fb_data->power_state == POWER_STATE_OFF)
+		|| fb_data->powering_down) {
+		spdc_powerup(fb_data);
+	}
+
+	/* This is a blocking call, so upon return PxP tx should be done */
+	ret = pxp_complete_update(fb_data, &hist_stat);
+	if (ret) {
+		dev_err(fb_data->dev, "Unable to complete PxP update task.\n");
+		mutex_unlock(&fb_data->pxp_mutex);
+		return ret;
+	}
+
+	if (use_temp_buf)
+		copy_to_next_buffer(fb_data, upd_data_list);
+
+	mutex_unlock(&fb_data->pxp_mutex);
+
+	/* Update waveform mode from PxP histogram results */
+	if (upd_desc_list->upd_data.waveform_mode == WAVEFORM_MODE_AUTO) {
+		if (hist_stat & 0x1)
+			upd_desc_list->upd_data.waveform_mode =
+				fb_data->wv_modes.mode_du;
+		else if (hist_stat & 0x2)
+			upd_desc_list->upd_data.waveform_mode =
+				fb_data->wv_modes.mode_gc4;
+		else if (hist_stat & 0x4)
+			upd_desc_list->upd_data.waveform_mode =
+				fb_data->wv_modes.mode_gc8;
+		else if (hist_stat & 0x8)
+			upd_desc_list->upd_data.waveform_mode =
+				fb_data->wv_modes.mode_gc16;
+		else
+			upd_desc_list->upd_data.waveform_mode =
+				fb_data->wv_modes.mode_gc32;
+
+		dev_dbg(fb_data->dev, "hist_stat = 0x%x, new waveform = 0x%x\n",
+			hist_stat, upd_desc_list->upd_data.waveform_mode);
+	}
+
+	return 0;
+}
+
+static bool spdc_submit_concur(mxc_spdc_t *fb_data,
+			struct update_desc_list *update_to_concur)
+{
+	struct mxcfb_update_data *a, *b;
+	struct mxcfb_rect *arect, *brect;
+	struct update_data_list *next_upd;
+	int i = 0;
+
+	a = &update_to_concur->upd_data;
+	arect = &update_to_concur->upd_data.update_region;
+
+	list_for_each_entry(next_upd,
+		&fb_data->upd_buf_preprocess_list, list) {
+		b = &next_upd->update_desc->upd_data;
+		brect = &next_upd->update_desc->upd_data.update_region;
+
+		/* Updates with different waveform
+		 * must be executed sequentially.
+		 */
+		if (a->waveform_mode != b->waveform_mode)
+			break;
+
+		/*
+		  * Concurrency update must has no overlay
+		  */
+		if (!(arect->left > (brect->left + brect->width) ||
+			brect->left > (arect->left + arect->width) ||
+			arect->top > (brect->top + brect->height) ||
+			brect->top > (arect->top + arect->height)))
+			break;
+
+		i++;
+	}
+
+	if (i != fb_data->upd_preprocess_num)
+		return false;
+
+	return true;
+}
+
+static int spdc_submit_merge(struct update_desc_list *upd_desc_list,
+				struct update_desc_list *update_to_merge)
+{
+	struct mxcfb_update_data *a, *b;
+	struct mxcfb_rect *arect, *brect;
+	struct mxcfb_rect combine;
+	bool use_flags = false;
+
+	a = &upd_desc_list->upd_data;
+	b = &update_to_merge->upd_data;
+	arect = &upd_desc_list->upd_data.update_region;
+	brect = &update_to_merge->upd_data.update_region;
+
+	/*
+	 * Updates with different flags must be executed sequentially.
+	 * Halt the merge process to ensure this.
+	 */
+	if (a->flags != b->flags) {
+		/*
+		 * Special exception: if update regions are identical,
+		 * we may be able to merge them.
+		 */
+		if ((arect->left != brect->left) ||
+			(arect->top != brect->top) ||
+			(arect->width != brect->width) ||
+			(arect->height != brect->height))
+			return MERGE_BLOCK;
+
+		use_flags = true;
+	}
+
+	if (a->waveform_mode != b->waveform_mode)
+		a->waveform_mode = WAVEFORM_MODE_AUTO;
+
+	if (arect->left > (brect->left + brect->width) ||
+		brect->left > (arect->left + arect->width) ||
+		arect->top > (brect->top + brect->height) ||
+		brect->top > (arect->top + arect->height))
+		return MERGE_FAIL;
+
+	combine.left = arect->left < brect->left ? arect->left : brect->left;
+	combine.top = arect->top < brect->top ? arect->top : brect->top;
+	combine.width = (arect->left + arect->width) >
+			(brect->left + brect->width) ?
+			(arect->left + arect->width - combine.left) :
+			(brect->left + brect->width - combine.left);
+	combine.height = (arect->top + arect->height) >
+			(brect->top + brect->height) ?
+			(arect->top + arect->height - combine.top) :
+			(brect->top + brect->height - combine.top);
+
+	*arect = combine;
+
+	/* Use flags of the later update */
+	if (use_flags)
+		a->flags = b->flags;
+
+	/* Merge markers */
+	list_splice_tail(&update_to_merge->upd_marker_list,
+		&upd_desc_list->upd_marker_list);
+
+	return MERGE_OK;
+
+}
+
+static void spdc_submit_work_func(struct work_struct *work)
+{
+	struct update_desc_list *next_desc, *temp_desc;
+	mxc_spdc_t *fb_data =
+		container_of(work, mxc_spdc_t, spdc_submit_work);
+	struct update_data_list *upd_data_list = NULL;
+	struct mxcfb_rect adj_update_region, *upd_region;
+	struct update_marker_data *current_marker;
+	bool end_merge = false;
+	bool is_transform;
+	u32 update_addr;
+
+	/* Protect access to buffer queues and to update HW */
+	mutex_lock(&fb_data->queue_mutex);
+
+	/* get a buffer from free list */
+	if (list_empty(&fb_data->upd_buf_free_list)) {
+		mutex_unlock(&fb_data->queue_mutex);
+		return;
+	}
+
+	if (fb_data->fresh_param.concur == SPDC_LUT_ACC_MODE) {
+		list_for_each_entry_safe(next_desc, temp_desc,
+			&fb_data->upd_pending_list, list) {
+
+			current_marker =
+			list_entry((&next_desc->upd_marker_list)->next,
+				struct update_marker_data, upd_list);
+
+			if (current_marker->update_marker) {
+				fb_data->submit_upd_sta = 0;
+				break;
+			}
+
+			/* require free buffer list */
+			if (list_empty(&fb_data->upd_buf_free_list)) {
+				dev_dbg(fb_data->dev,
+					"buf free list is empty\n");
+				break;
+			}
+
+			upd_data_list =
+			list_entry(fb_data->upd_buf_free_list.next,
+				struct update_data_list, list);
+			upd_data_list->update_desc = next_desc;
+
+			if (!is_preprocess_list_full(fb_data)) {
+				if (fb_data->cur_update == NULL &&
+					!fb_data->upd_preprocess_num)
+					list_del_init(&next_desc->list);
+				else if (spdc_submit_concur(fb_data,
+						next_desc)) {
+					list_del_init(&next_desc->list);
+					list_add_tail(&upd_data_list->list,
+					&fb_data->upd_buf_preprocess_list);
+					fb_data->upd_preprocess_num++;
+					fb_data->submit_upd_sta =
+							SPDC_CONCUR_UPD;
+				} else
+					break;
+			} else
+				break;
+
+			/* submit to pxp process */
+			list_del_init(&upd_data_list->list);
+			goto pxp_process;
+		}
+
+		upd_data_list = NULL;
+		if (fb_data->submit_upd_sta == SPDC_CONCUR_UPD)
+			fb_data->submit_upd_sta |= SPDC_QUEUE_UPD;
+		else
+			fb_data->submit_upd_sta = SPDC_QUEUE_UPD;
+	}
+
+	list_for_each_entry_safe(next_desc, temp_desc,
+			&fb_data->upd_pending_list, list) {
+
+		if (!upd_data_list) {
+
+			if (list_empty(&fb_data->upd_buf_free_list)) {
+				dev_dbg(fb_data->dev,
+					"buf_free_list is empty\n");
+				break;
+			}
+			upd_data_list =
+				list_entry(fb_data->upd_buf_free_list.next,
+					struct update_data_list, list);
+			list_del_init(&upd_data_list->list);
+			upd_data_list->update_desc = next_desc;
+			list_del_init(&next_desc->list);
+
+			if (fb_data->upd_scheme == UPDATE_SCHEME_QUEUE)
+				break;
+		} else {
+			switch (spdc_submit_merge(upd_data_list->update_desc,
+					next_desc)) {
+			case MERGE_OK:
+				dev_dbg(fb_data->dev,
+					"Update merged [queue]\n");
+				list_del_init(&next_desc->list);
+				kfree(next_desc);
+				break;
+			case MERGE_FAIL:
+				dev_dbg(fb_data->dev,
+					"Update not merged [queue]\n");
+				break;
+			case MERGE_BLOCK:
+				dev_dbg(fb_data->dev,
+					"Merge blocked [collision]\n");
+				end_merge = true;
+				break;
+			}
+
+			if (end_merge)
+				break;
+		}
+	}
+
+	/* Is update list empty? */
+	if (!upd_data_list) {
+		mutex_unlock(&fb_data->queue_mutex);
+		return;
+	}
+
+pxp_process:
+	/*
+	 * If no processing required, skip update processing
+	 * No processing means:
+	 *   - FB unrotated
+	 *   - FB pixel format = 4-bit grayscale
+	 *   - No look-up transformations (inversion, posterization, etc.)
+	 */
+	is_transform = upd_data_list->update_desc->upd_data.flags &
+		(EPDC_FLAG_ENABLE_INVERSION |
+		EPDC_FLAG_FORCE_MONOCHROME | EPDC_FLAG_USE_CMAP) ?
+		true : false;
+	if ((fb_data->spdc_fb_var.rotate == FB_ROTATE_UR) &&
+		(fb_data->spdc_fb_var.grayscale == GRAYSCALE_4BIT) &&
+		!is_transform) {
+
+		/* If needed, enable SPDC HW while ePxP is processing */
+		if ((fb_data->power_state == POWER_STATE_OFF)
+			|| fb_data->powering_down)
+			spdc_powerup(fb_data);
+
+		/*
+		 * Set update buffer pointer to the start of
+		 * the update region in the frame buffer.
+		 */
+		upd_region =
+			&upd_data_list->update_desc->upd_data.update_region;
+		update_addr = fb_data->info.fix.smem_start +
+			((upd_region->top * fb_data->info.var.xres_virtual) +
+			upd_region->left) * fb_data->info.var.bits_per_pixel/8;
+	} else {
+
+		/* Select from PxP output buffers */
+		upd_data_list->phys_addr =
+			fb_data->phys_addr_updbuf[fb_data->upd_buffer_num];
+		upd_data_list->virt_addr =
+			fb_data->virt_addr_updbuf[fb_data->upd_buffer_num];
+		fb_data->upd_buffer_num++;
+		if (fb_data->upd_buffer_num > fb_data->max_num_buffers-1)
+			fb_data->upd_buffer_num = 0;
+		dev_dbg(fb_data->dev,
+			"pxp out addr:0x%x\n", upd_data_list->phys_addr);
+
+		/* Release buffer queues */
+		mutex_unlock(&fb_data->queue_mutex);
+
+		/* Perform PXP processing - SPDC power will also be enabled */
+		if (spdc_process_update(upd_data_list, fb_data)) {
+
+			dev_dbg(fb_data->dev, "PXP processing error.\n");
+			/* Protect access to buffer queues and to update HW */
+			mutex_lock(&fb_data->queue_mutex);
+			list_del_init(&upd_data_list->update_desc->list);
+			kfree(upd_data_list->update_desc);
+			upd_data_list->update_desc = NULL;
+
+			/* Add to free buffer list */
+			list_add_tail(&upd_data_list->list,
+				&fb_data->upd_buf_free_list);
+			/* Release buffer queues */
+			mutex_unlock(&fb_data->queue_mutex);
+			return;
+		}
+
+		/* Protect access to buffer queues and to update HW */
+		mutex_lock(&fb_data->queue_mutex);
+
+		/* output Y4 format */
+		update_addr = upd_data_list->phys_addr +
+			+ (upd_data_list->update_desc->spdc_offs / 2);
+	}
+
+	/* Get rotation-adjusted coordinates */
+	adjust_coordinates(fb_data->spdc_fb_var.xres,
+		fb_data->spdc_fb_var.yres, fb_data->spdc_fb_var.rotate,
+		&upd_data_list->update_desc->upd_data.update_region,
+		&adj_update_region);
+
+	/*
+	 * Is the working buffer idle?
+	 * If the working buffer is busy, we must wait for the resource
+	 * to become free.
+	 */
+	if (fb_data->cur_update != NULL &&
+	fb_data->submit_upd_sta != SPDC_CONCUR_UPD) {
+		/* Initialize event signalling an update resource is free */
+		init_completion(&fb_data->update_res_free);
+
+		fb_data->waiting_for_wb = true;
+
+		/* Leave spinlock while waiting for WB to complete */
+		mutex_unlock(&fb_data->queue_mutex);
+		wait_for_completion(&fb_data->update_res_free);
+		mutex_lock(&fb_data->queue_mutex);
+	}
+
+	if (fb_data->submit_upd_sta != SPDC_CONCUR_UPD)
+		fb_data->cur_update = upd_data_list;
+
+	/* program SPDC register and trigger to process buffer*/
+	fb_data->fresh_param.buf_addr.next_buf_phys_addr = update_addr;
+
+	fb_data->fresh_param.update_region.left = adj_update_region.left;
+	fb_data->fresh_param.update_region.top = adj_update_region.top;
+	fb_data->fresh_param.update_region.width = adj_update_region.width;
+	fb_data->fresh_param.update_region.height = adj_update_region.height;
+	fb_data->fresh_param.temper = upd_data_list->update_desc->upd_data.temp;
+	fb_data->fresh_param.wave_mode =
+		upd_data_list->update_desc->upd_data.waveform_mode;
+
+	spdc_submit_update(fb_data);
+
+	/* Release buffer queues */
+	mutex_unlock(&fb_data->queue_mutex);
+}
+
+int mxc_spdc_fb_send_update(struct mxcfb_update_data *upd_data,
+				   struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = info ?
+		(mxc_spdc_t *)info:g_fb_data;
+	struct update_data_list *upd_data_list = NULL;
+	struct mxcfb_rect *screen_upd_region; /* Region on screen to update */
+	struct update_desc_list *upd_desc;
+	struct update_marker_data *marker_data;
+	int ret;
+
+	/* Has SPDC HW been initialized? */
+	if (!fb_data->hw_ready) {
+		/* Throw message if we are not mid-initialization */
+		if (!fb_data->hw_initializing)
+			dev_err(fb_data->dev, "Display HW not properly"
+				"initialized. Aborting update.\n");
+		return -EPERM;
+	}
+
+	if ((upd_data->waveform_mode > SPDC_WAV_MODE_5) &&
+		(upd_data->waveform_mode != WAVEFORM_MODE_AUTO)) {
+		dev_err(fb_data->dev,
+			"Update waveform mode 0x%x is invalid."
+			"  Aborting update.\n",
+			upd_data->waveform_mode);
+		return -EINVAL;
+	}
+	if ((upd_data->update_region.left + upd_data->update_region.width >
+		fb_data->spdc_fb_var.xres + 1) ||
+		(upd_data->update_region.top + upd_data->update_region.height >
+		fb_data->spdc_fb_var.yres + 1)) {
+		dev_err(fb_data->dev,
+			"Update region is outside bounds of framebuffer."
+			"Aborting update.\n");
+		return -EINVAL;
+	}
+	if (upd_data->flags & EPDC_FLAG_USE_ALT_BUFFER) {
+		if ((upd_data->update_region.width !=
+			upd_data->alt_buffer_data.alt_update_region.width) ||
+			(upd_data->update_region.height !=
+			upd_data->alt_buffer_data.alt_update_region.height)) {
+			dev_err(fb_data->dev,
+				"Alternate update region dimensions must "
+				"match screen update region dimensions.\n");
+			return -EINVAL;
+		}
+		/* Validate physical address parameter */
+		if ((upd_data->alt_buffer_data.phys_addr <
+			fb_data->info.fix.smem_start) ||
+			(upd_data->alt_buffer_data.phys_addr >
+			fb_data->info.fix.smem_start + fb_data->map_size)) {
+			dev_err(fb_data->dev,
+				"Invalid physical address for alternate "
+				"buffer.  Aborting update...\n");
+			return -EINVAL;
+		}
+	}
+
+	mutex_lock(&fb_data->queue_mutex);
+
+	/*
+	 * If we are waiting to go into suspend, or the FB is blanked,
+	 * we do not accept new updates
+	 */
+	if (fb_data->waiting_for_idle) {
+		dev_dbg(fb_data->dev, "SPDC not active."
+			"Update request abort.\n");
+		mutex_unlock(&fb_data->queue_mutex);
+		return -EPERM;
+	}
+
+	if (fb_data->upd_scheme == UPDATE_SCHEME_SNAPSHOT) {
+		int count = 0;
+		struct update_data_list *plist;
+
+		/* Count buffers in free buffer list */
+		list_for_each_entry(plist, &fb_data->upd_buf_free_list, list)
+			count++;
+
+		/* Use count to determine if we have enough
+		 * free buffers to handle this update request */
+		if (count + fb_data->max_num_buffers
+			<= fb_data->max_num_updates) {
+			dev_err(fb_data->dev,
+				"No free intermediate buffers available.\n");
+			mutex_unlock(&fb_data->queue_mutex);
+			return -ENOMEM;
+		}
+
+		/* Grab first available buffer and delete from the free list */
+		upd_data_list =
+		list_entry(fb_data->upd_buf_free_list.next,
+			   struct update_data_list, list);
+
+		list_del_init(&upd_data_list->list);
+	}
+
+	/*
+	 * Create new update data structure, fill it with new update
+	 * data and add it to the list of pending updates
+	 */
+	upd_desc = kzalloc(sizeof(struct update_desc_list), GFP_KERNEL);
+	if (!upd_desc) {
+		dev_err(fb_data->dev,
+			"Insufficient system memory for update! Aborting.\n");
+		if (fb_data->upd_scheme == UPDATE_SCHEME_SNAPSHOT) {
+			list_add(&upd_data_list->list,
+				&fb_data->upd_buf_free_list);
+		}
+		mutex_unlock(&fb_data->queue_mutex);
+		return -EPERM;
+	}
+	/* Initialize per-update marker list */
+	INIT_LIST_HEAD(&upd_desc->upd_marker_list);
+	upd_desc->upd_data = *upd_data;
+	list_add_tail(&upd_desc->list, &fb_data->upd_pending_list);
+
+	/* If marker specified, associate it with a completion */
+	if (upd_data->update_marker != 0) {
+
+		/* Allocate new update marker and set it up */
+		marker_data = kzalloc(sizeof(struct update_marker_data),
+				GFP_KERNEL);
+		if (!marker_data) {
+			dev_err(fb_data->dev, "No memory for marker!\n");
+			mutex_unlock(&fb_data->queue_mutex);
+			return -ENOMEM;
+		}
+		list_add_tail(&marker_data->upd_list,
+			&upd_desc->upd_marker_list);
+		marker_data->update_marker = upd_data->update_marker;
+		init_completion(&marker_data->update_completion);
+
+		/* Add marker to master marker list */
+		list_add_tail(&marker_data->full_list,
+			&fb_data->full_marker_list);
+	}
+
+	if (fb_data->upd_scheme != UPDATE_SCHEME_SNAPSHOT) {
+		/* Queued update scheme processing */
+
+		mutex_unlock(&fb_data->queue_mutex);
+
+		/* Signal workqueue to handle new update */
+		queue_work(fb_data->spdc_submit_workqueue,
+			&fb_data->spdc_submit_work);
+
+		return 0;
+	}
+
+	/* Set descriptor for current update, delete from pending list */
+	upd_data_list->update_desc = upd_desc;
+	list_del_init(&upd_desc->list);
+
+	mutex_unlock(&fb_data->queue_mutex);
+
+	/*
+	 * Hold on to original screen update region, which we
+	 * will ultimately use when telling SPDC where to update on panel
+	 */
+	screen_upd_region = &upd_desc->upd_data.update_region;
+
+	/* Select from PxP output buffers */
+	upd_data_list->phys_addr =
+		fb_data->phys_addr_updbuf[fb_data->upd_buffer_num];
+	upd_data_list->virt_addr =
+		fb_data->virt_addr_updbuf[fb_data->upd_buffer_num];
+	fb_data->upd_buffer_num++;
+	if (fb_data->upd_buffer_num > fb_data->max_num_buffers-1)
+		fb_data->upd_buffer_num = 0;
+
+	ret = spdc_process_update(upd_data_list, fb_data);
+	if (ret) {
+		mutex_unlock(&fb_data->pxp_mutex);
+		return ret;
+	}
+
+	/* Pass selected waveform mode back to user */
+	upd_data->waveform_mode = upd_desc->upd_data.waveform_mode;
+
+	/* Get rotation-adjusted coordinates */
+	adjust_coordinates(fb_data->spdc_fb_var.xres,
+		fb_data->spdc_fb_var.yres, fb_data->spdc_fb_var.rotate,
+		&upd_desc->upd_data.update_region, NULL);
+
+	/* Grab lock for queue manipulation and update submission */
+	mutex_lock(&fb_data->queue_mutex);
+
+	/*
+	 * Is the working buffer idle?
+	 * If either the working buffer is busy, or there are no LUTs available,
+	 * then we return and let the ISR handle the update later
+	 */
+	if (fb_data->cur_update != NULL) {
+
+		/* Add processed Y buffer to update list */
+		list_add_tail(&upd_data_list->list, &fb_data->upd_buf_queue);
+
+		/* Return and allow the update to be submitted by the ISR. */
+		mutex_unlock(&fb_data->queue_mutex);
+		return 0;
+	}
+
+	/* Save current update */
+	fb_data->cur_update = upd_data_list;
+
+	/* program SPDC register and trigger to process buffer*/
+	fb_data->fresh_param.buf_addr.next_buf_phys_addr =
+	upd_data_list->phys_addr + (upd_data_list->update_desc->spdc_offs / 2);
+
+	fb_data->fresh_param.update_region.left = screen_upd_region->left;
+	fb_data->fresh_param.update_region.top = screen_upd_region->top;
+	fb_data->fresh_param.update_region.width = screen_upd_region->width;
+	fb_data->fresh_param.update_region.height = screen_upd_region->height;
+	fb_data->fresh_param.temper = upd_desc->upd_data.temp;
+	fb_data->fresh_param.wave_mode = upd_desc->upd_data.waveform_mode;
+	spdc_submit_update(fb_data);
+
+	mutex_unlock(&fb_data->queue_mutex);
+	return 0;
+}
+EXPORT_SYMBOL(mxc_spdc_fb_send_update);
+
+/*
+ * return 0 : spdc update is update
+ */
+int
+mxc_spdc_fb_wait_update_complete(struct mxcfb_update_marker_data *marker_data,
+				struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = info ?
+		(mxc_spdc_t *)info:g_fb_data;
+	struct update_marker_data *next_marker;
+	struct update_marker_data *temp;
+	bool marker_found = false;
+	int ret = 0;
+
+	/* 0 is an invalid update_marker value */
+	if (marker_data->update_marker == 0)
+		return -EINVAL;
+
+	/*
+	 * Find completion associated with update_marker requested.
+	 * Note: If update completed already, marker will have been
+	 * cleared, it won't be found, and function will just return.
+	 */
+
+	/* Grab queue lock to protect access to marker list */
+	mutex_lock(&fb_data->queue_mutex);
+
+	list_for_each_entry_safe(next_marker, temp,
+		&fb_data->full_marker_list, full_list) {
+		if (next_marker->update_marker == marker_data->update_marker) {
+			dev_dbg(fb_data->dev, "Waiting for marker %d\n",
+				marker_data->update_marker);
+			next_marker->waiting = true;
+			marker_found = true;
+			break;
+		}
+	}
+
+	mutex_unlock(&fb_data->queue_mutex);
+
+	/*
+	 * If marker not found, it has either been signalled already
+	 * or the update request failed.  In either case, just return.
+	 */
+	if (!marker_found)
+		return ret;
+
+	ret = wait_for_completion_timeout(&next_marker->update_completion,
+						msecs_to_jiffies(8000));
+	if (!ret) {
+		dev_err(fb_data->dev,
+			"Timed out waiting for update completion\n");
+		return -ETIMEDOUT;
+	}
+
+	/** since SPDC don't support auto collision detect,
+	 *  there alway returns no collision
+	 */
+	marker_data->collision_test = false;
+
+	/* Free update marker object */
+	kfree(next_marker);
+
+	return ret;
+}
+EXPORT_SYMBOL(mxc_spdc_fb_wait_update_complete);
+
+int mxc_spdc_fb_set_pwrdown_delay(u32 pwrdown_delay,
+				struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = info ?
+		(mxc_spdc_t *)info:g_fb_data;
+
+	fb_data->pwrdown_delay = pwrdown_delay;
+
+	return 0;
+}
+EXPORT_SYMBOL(mxc_spdc_fb_set_pwrdown_delay);
+
+int mxc_spdc_get_pwrdown_delay(struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = info ?
+		(mxc_spdc_t *)info:g_fb_data;
+
+	return fb_data->pwrdown_delay;
+}
+EXPORT_SYMBOL(mxc_spdc_get_pwrdown_delay);
+
+static int
+mxc_spdc_fb_ioctl(struct fb_info *info, unsigned int cmd, unsigned long arg)
+{
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	void __user *argp = (void __user *)arg;
+	int ret = -EINVAL;
+
+	dev_dbg(fb_data->dev, "cmd = %08X, arg = %08X\n", cmd, (u32)arg);
+
+	switch (cmd) {
+	case MXCFB_SET_WAVEFORM_MODES:
+		{
+			struct mxcfb_waveform_modes modes;
+			if (!copy_from_user(&modes, argp, sizeof(modes))) {
+				mxc_spdc_fb_set_waveform_modes(&modes, info);
+				ret = 0;
+			}
+			break;
+		}
+	case MXCFB_SET_TEMPERATURE:
+		{
+			int temperature;
+			if (!get_user(temperature, (int32_t __user *) arg))
+				ret = mxc_spdc_fb_set_temperature(temperature,
+					info);
+			break;
+		}
+	case MXCFB_SET_AUTO_UPDATE_MODE:
+		{
+			u32 auto_mode = 0;
+			if (!get_user(auto_mode, (__u32 __user *) arg))
+				ret = mxc_spdc_fb_set_auto_update(auto_mode,
+					info);
+			break;
+		}
+	case MXCFB_SET_UPDATE_SCHEME:
+		{
+			u32 upd_scheme = 0;
+			if (!get_user(upd_scheme, (__u32 __user *) arg))
+				ret = mxc_spdc_fb_set_upd_scheme(upd_scheme,
+					info);
+			break;
+		}
+	case MXCFB_SEND_UPDATE:
+		{
+			struct mxcfb_update_data upd_data;
+			if (!copy_from_user(&upd_data, argp,
+				sizeof(upd_data))) {
+				ret = mxc_spdc_fb_send_update(&upd_data, info);
+				if (ret == 0 && copy_to_user(argp, &upd_data,
+					sizeof(upd_data)))
+					ret = -EFAULT;
+			} else {
+				ret = -EFAULT;
+			}
+
+			break;
+		}
+	case MXCFB_WAIT_FOR_UPDATE_COMPLETE:
+		{
+			struct mxcfb_update_marker_data upd_marker_data;
+			if (!copy_from_user(&upd_marker_data, argp,
+				sizeof(upd_marker_data))) {
+				ret = mxc_spdc_fb_wait_update_complete(
+					&upd_marker_data, info);
+				if (copy_to_user(argp, &upd_marker_data,
+					sizeof(upd_marker_data)))
+					ret = -EFAULT;
+			} else {
+				ret = -EFAULT;
+			}
+
+			break;
+		}
+	case MXCFB_SET_PWRDOWN_DELAY:
+		{
+			int delay = 0;
+			if (!get_user(delay, (__u32 __user *) arg))
+				ret =
+				mxc_spdc_fb_set_pwrdown_delay(delay, info);
+			break;
+		}
+	case MXCFB_GET_PWRDOWN_DELAY:
+		{
+			int pwrdown_delay = mxc_spdc_get_pwrdown_delay(info);
+			if (put_user(pwrdown_delay, (int __user *)arg))
+				ret = -EFAULT;
+			break;
+		}
+	default:
+		dev_err(fb_data->dev, "IOCTL_CMD: not such command\n");
+		return -ENOTTY;
+	}
+
+	return ret;
+}
+
+static void mxc_spdc_fb_update_pages(mxc_spdc_t *fb_data, u16 y1, u16 y2)
+{
+	struct mxcfb_update_data update;
+
+	/* Do partial screen update, Update full horizontal lines */
+	update.update_region.left = 0;
+	update.update_region.width = fb_data->spdc_fb_var.xres;
+	update.update_region.top = y1;
+	update.update_region.height = y2 - y1;
+	update.waveform_mode = WAVEFORM_MODE_AUTO;
+	update.update_mode = UPDATE_MODE_FULL;
+	update.update_marker = 0;
+	update.temp = SPDC_DEFAULT_TEMP;
+	update.flags = 0;
+
+	mxc_spdc_fb_send_update(&update, &fb_data->info);
+}
+
+/* this is called back from the deferred io workqueue */
+static void mxc_spdc_fb_deferred_io(struct fb_info *info,
+				struct list_head *pagelist)
+{
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	struct page *page;
+	unsigned long beg, end;
+	int y1, y2, miny, maxy;
+
+	if (fb_data->auto_mode != AUTO_UPDATE_MODE_AUTOMATIC_MODE)
+		return;
+
+	miny = INT_MAX;
+	maxy = 0;
+	list_for_each_entry(page, pagelist, lru) {
+		beg = page->index << PAGE_SHIFT;
+		end = beg + PAGE_SIZE - 1;
+		y1 = beg / info->fix.line_length;
+		y2 = end / info->fix.line_length;
+		if (y2 >= fb_data->spdc_fb_var.yres)
+			y2 = fb_data->spdc_fb_var.yres - 1;
+		if (miny > y1)
+			miny = y1;
+		if (maxy < y2)
+			maxy = y2;
+	}
+
+	mxc_spdc_fb_update_pages(fb_data, miny, maxy);
+}
+
+static int mxc_spdc_fb_blank(int blank, struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	int ret = 0;
+
+	dev_dbg(fb_data->dev, "blank = %d\n", blank);
+
+	if (fb_data->blank == blank)
+		return 0;
+
+	fb_data->blank = blank;
+	switch (blank) {
+	case FB_BLANK_POWERDOWN:
+		mxc_spdc_fb_flush_updates(fb_data);
+		/* Wait for powerdown */
+		mutex_lock(&fb_data->power_mutex);
+		if ((fb_data->power_state == POWER_STATE_ON) &&
+			(fb_data->pwrdown_delay == FB_POWERDOWN_DISABLE)) {
+
+			/* Powerdown disabled, so we disable SPDC manually */
+			int count = 0;
+			int sleep_ms = 10;
+
+			mutex_unlock(&fb_data->power_mutex);
+
+			/* If any active updates, wait for them to complete */
+			while (fb_data->updates_active) {
+				/* Timeout after 1 sec */
+				if ((count * sleep_ms) > 1000)
+					break;
+				msleep(sleep_ms);
+				count++;
+			}
+
+			fb_data->powering_down = true;
+			spdc_powerdown(fb_data);
+		} else if (fb_data->power_state != POWER_STATE_OFF) {
+			fb_data->wait_for_powerdown = true;
+			init_completion(&fb_data->powerdown_compl);
+			mutex_unlock(&fb_data->power_mutex);
+			ret =
+			wait_for_completion_timeout(&fb_data->powerdown_compl,
+				msecs_to_jiffies(5000));
+			if (!ret) {
+				dev_err(fb_data->dev,
+					"No powerdown received!\n");
+				return -ETIMEDOUT;
+			}
+		} else
+			mutex_unlock(&fb_data->power_mutex);
+		break;
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_NORMAL:
+		mxc_spdc_fb_flush_updates(fb_data);
+		break;
+	}
+
+	return ret;
+}
+
+static int mxc_spdc_fb_pan_display(struct fb_var_screeninfo *var,
+				   struct fb_info *info)
+{
+	mxc_spdc_t *fb_data = info ?
+		(mxc_spdc_t *)info:g_fb_data;
+	u_int y_bottom;
+
+	dev_dbg(info->device, "%s: var->yoffset %d, info->var.yoffset %d\n",
+		 __func__, var->yoffset, info->var.yoffset);
+	/* check if var is valid; also, xpan is not supported */
+	if (!var || (var->xoffset != info->var.xoffset) ||
+	(var->yoffset + var->yres > var->yres_virtual)) {
+		dev_dbg(info->device, "x panning not supported\n");
+		return -EINVAL;
+	}
+
+	if ((fb_data->spdc_fb_var.xoffset == var->xoffset) &&
+		(fb_data->spdc_fb_var.yoffset == var->yoffset))
+		return 0;	/* No change, do nothing */
+
+	y_bottom = var->yoffset;
+
+	if (!(var->vmode & FB_VMODE_YWRAP))
+		y_bottom += var->yres;
+
+	if (y_bottom > info->var.yres_virtual)
+		return -EINVAL;
+
+	mutex_lock(&fb_data->queue_mutex);
+
+	fb_data->fb_offset = (var->yoffset * var->xres_virtual + var->xoffset)
+		* (var->bits_per_pixel) / 8;
+
+	fb_data->spdc_fb_var.xoffset = var->xoffset;
+	fb_data->spdc_fb_var.yoffset = var->yoffset;
+
+	if (var->vmode & FB_VMODE_YWRAP)
+		info->var.vmode |= FB_VMODE_YWRAP;
+	else
+		info->var.vmode &= ~FB_VMODE_YWRAP;
+
+	mutex_unlock(&fb_data->queue_mutex);
+
+	return 0;
+}
+
+static struct fb_ops mxc_spdc_fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var = mxc_spdc_fb_check_var,
+	.fb_set_par = mxc_spdc_fb_set_par,
+	.fb_setcmap = mxc_spdc_fb_setcmap,
+	.fb_setcolreg = mxc_spdc_fb_setcolreg,
+	.fb_pan_display = mxc_spdc_fb_pan_display,
+	.fb_ioctl = mxc_spdc_fb_ioctl,
+	.fb_mmap = mxc_spdc_fb_mmap,
+	.fb_blank = mxc_spdc_fb_blank,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+};
+
+static struct fb_deferred_io mxc_spdc_fb_defio = {
+	.delay = HZ,
+	.deferred_io = mxc_spdc_fb_deferred_io,
+};
+
+static void spdc_done_work_func(struct work_struct *work)
+{
+	mxc_spdc_t *fb_data =
+		container_of(work, mxc_spdc_t, spdc_done_work.work);
+	spdc_powerdown(fb_data);
+}
+
+static irqreturn_t mxc_spdc_irq_handler(int irq, void *dev_id)
+{
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)dev_id;
+	ulong flags;
+	u32 int_events;
+	irqreturn_t ret = IRQ_NONE;
+
+	spin_lock_irqsave(&fb_data->lock, flags);
+
+	int_events = spdc_get_intr_stat(fb_data);
+	dev_dbg(fb_data->dev, "spdc int:%x\n", int_events);
+
+	if (int_events & SPDC_IRQ_STA_ERR) {
+		ret = IRQ_HANDLED;
+		spdc_intr_stat_clear(fb_data, SPDC_IRQ_STA_ERR);
+		dev_err(fb_data->dev, "Error IRQ\n");
+		return ret;
+	}
+
+	/*
+	 * If we just completed one-time panel init, bypass
+	 * queue handling, clear interrupt and return
+	 */
+	if (fb_data->operation_mode &&
+		(int_events & SPDC_IRQ_STA_FRAME_UPDATE)) {
+		mutex_lock(&fb_data->queue_mutex);
+		fb_data->updates_active = false;
+		complete(&fb_data->update_finish);
+		spdc_intr_stat_clear(fb_data, SPDC_IRQ_STA_FRAME_UPDATE);
+
+		if ((fb_data->operation_mode == SPDC_DEEP_REFRESH) ||
+			fb_data->is_deep_fresh)
+			fb_data->is_deep_fresh = false;
+		else
+			fb_data->operation_mode = SPDC_NO_OPERATION;
+
+		mutex_unlock(&fb_data->queue_mutex);
+
+		return IRQ_HANDLED;
+	}
+
+	/* waveform loading to SPDC  from memory */
+	if (int_events & SPDC_IRQ_STA_LUT_DOWNLOAD) {
+		if (is_lut_checksum_ok(fb_data)) {
+			complete(&fb_data->lut_down);
+			spdc_intr_stat_clear(fb_data,
+				SPDC_IRQ_STA_LUT_DOWNLOAD);
+		} else
+			dev_dbg(fb_data->dev, "Lut checksum is err!\n");
+
+		return IRQ_HANDLED;
+	}
+
+	/* SPDC init setting IRQ */
+	if (int_events & SPDC_IRQ_STA_TCON_INIT) {
+		complete(&fb_data->init_finish);
+		spdc_intr_stat_clear(fb_data, SPDC_IRQ_STA_TCON_INIT);
+
+		return IRQ_HANDLED;
+	}
+
+	spin_unlock_irqrestore(&fb_data->lock, flags);
+
+	if (spdc_is_update_finish(fb_data)) {
+		/* clear interrupt status */
+		spdc_intr_stat_clear(fb_data, SPDC_IRQ_STA_FRAME_UPDATE);
+		queue_work(fb_data->spdc_intr_workqueue,
+			&fb_data->spdc_intr_work);
+	}
+
+	 return ret;
+}
+
+static void spdc_intr_work_func(struct work_struct *work)
+{
+	mxc_spdc_t *fb_data =
+		container_of(work, mxc_spdc_t, spdc_intr_work);
+	struct mxcfb_rect *next_upd_region;
+	struct update_marker_data *next_marker;
+	struct update_marker_data *temp;
+	struct update_data_list *next_upd, *temp_upd;
+
+	/* Protect access to buffer queues and to update HW */
+	mutex_lock(&fb_data->queue_mutex);
+
+	/* Check to see if all updates have completed */
+	if (list_empty(&fb_data->upd_pending_list) &&
+		is_free_list_full(fb_data) &&
+		(fb_data->cur_update == NULL)) {
+
+		fb_data->updates_active = false;
+
+		if (fb_data->pwrdown_delay != FB_POWERDOWN_DISABLE) {
+			/*
+			 * Set variable to prevent overlapping
+			 * enable/disable requests
+			 */
+			fb_data->powering_down = true;
+
+			/* Schedule task to disable SPDC HW until next update */
+			schedule_delayed_work(&fb_data->spdc_done_work,
+				msecs_to_jiffies(fb_data->pwrdown_delay));
+
+			/* Reset counter to reduce chance of overflow */
+			fb_data->order_cnt = 0;
+		}
+
+		if (fb_data->waiting_for_idle)
+			complete(&fb_data->updates_done);
+	}
+
+	if (mxc_spdc_device_is_busy(fb_data)) {
+		/* Can't submit another update until SPDC is idle */
+		mutex_unlock(&fb_data->queue_mutex);
+		return;
+	}
+
+	/*
+	 * Were we waiting on working buffer?
+	 * If so, update queues and check for collisions
+	 */
+	if (fb_data->cur_update != NULL) {
+		list_for_each_entry_safe(next_marker, temp,
+			&fb_data->cur_update->update_desc->upd_marker_list,
+			upd_list) {
+
+			/* Del from per-update & full list */
+			list_del_init(&next_marker->upd_list);
+			list_del_init(&next_marker->full_list);
+
+			/* Signal completion of update */
+			dev_dbg(fb_data->dev,
+				"Signaling marker %d\n",
+				next_marker->update_marker);
+			if (next_marker->waiting)
+				complete(&next_marker->update_completion);
+			else
+				kfree(next_marker);
+		}
+
+		/* Free marker list and update descriptor */
+		kfree(fb_data->cur_update->update_desc);
+
+		/* Add to free buffer list */
+		list_add_tail(&fb_data->cur_update->list,
+			 &fb_data->upd_buf_free_list);
+
+		if (fb_data->submit_upd_sta & SPDC_CONCUR_UPD) {
+			fb_data->upd_preprocess_num = 0;
+			fb_data->submit_upd_sta &= (~SPDC_CONCUR_UPD);
+		}
+
+		/* free ACC update list */
+		list_for_each_entry_safe(next_upd, temp_upd,
+			&fb_data->upd_buf_preprocess_list, list) {
+			next_marker = list_entry(
+			(&next_upd->update_desc->upd_marker_list)->next,
+				struct update_marker_data, upd_list);
+
+			list_del_init(&next_marker->upd_list);
+			list_del_init(&next_marker->full_list);
+
+			/* Signal completion of update */
+			dev_dbg(fb_data->dev,
+				"Signaling marker %d\n",
+				next_marker->update_marker);
+
+			if (next_marker->waiting)
+				complete(&next_marker->update_completion);
+			else
+				kfree(next_marker);
+
+
+			list_del_init(&next_upd->list);
+			/* Add to free buffer list */
+			list_add_tail(&next_upd->list,
+				&fb_data->upd_buf_free_list);
+			kfree(next_upd->update_desc);
+		}
+
+		/* Check to see if all updates have completed */
+		if (list_empty(&fb_data->upd_pending_list) &&
+			is_free_list_full(fb_data)) {
+
+			fb_data->updates_active = false;
+
+			if (fb_data->pwrdown_delay !=
+					FB_POWERDOWN_DISABLE) {
+				/*
+				 * Set variable to prevent overlapping
+				 * enable/disable requests
+				 */
+				fb_data->powering_down = true;
+
+				/* Schedule SPDC disable */
+				schedule_delayed_work(&fb_data->spdc_done_work,
+				msecs_to_jiffies(fb_data->pwrdown_delay));
+
+				/* Reset counter to reduce chance of overflow */
+				fb_data->order_cnt = 0;
+			}
+
+			if (fb_data->waiting_for_idle)
+				complete(&fb_data->updates_done);
+		}
+
+		/* Signal completion if submit workqueue was waiting on WB */
+		if (fb_data->waiting_for_wb) {
+			dev_dbg(fb_data->dev, "free update_res_free\n");
+			complete(&fb_data->update_res_free);
+			fb_data->waiting_for_wb = false;
+		}
+
+		/* Clear current update */
+		fb_data->cur_update = NULL;
+	}
+
+	if (fb_data->upd_scheme != UPDATE_SCHEME_SNAPSHOT) {
+		/* Queued update scheme processing */
+		/* Schedule task to submit collision and pending update */
+		if (!fb_data->powering_down)
+			queue_work(fb_data->spdc_submit_workqueue,
+				&fb_data->spdc_submit_work);
+
+		/* Release buffer queues */
+		mutex_unlock(&fb_data->queue_mutex);
+		return;
+	}
+
+	/* Snapshot update scheme processing */
+	if (list_empty(&fb_data->upd_buf_queue)) {
+		dev_dbg(fb_data->dev, "No pending updates.\n");
+
+		/* No updates pending, so we are done */
+		mutex_unlock(&fb_data->queue_mutex);
+		return;
+	} else {
+		dev_dbg(fb_data->dev, "Found a pending update!\n");
+
+		/* Process next item in update list */
+		fb_data->cur_update =
+			list_entry(fb_data->upd_buf_queue.next,
+				struct update_data_list, list);
+		list_del_init(&fb_data->cur_update->list);
+	}
+
+	/* program SPDC register and trigger to process buffer*/
+	next_upd_region =
+		&fb_data->cur_update->update_desc->upd_data.update_region;
+	fb_data->fresh_param.buf_addr.next_buf_phys_addr =
+		fb_data->cur_update->phys_addr +
+		(fb_data->cur_update->update_desc->spdc_offs / 2);
+
+	fb_data->fresh_param.update_region.left = next_upd_region->left;
+	fb_data->fresh_param.update_region.top = next_upd_region->top;
+	fb_data->fresh_param.update_region.width = next_upd_region->width;
+	fb_data->fresh_param.update_region.height = next_upd_region->height;
+	fb_data->fresh_param.temper =
+		fb_data->cur_update->update_desc->upd_data.temp;
+	fb_data->fresh_param.wave_mode =
+		fb_data->cur_update->update_desc->upd_data.waveform_mode;
+	spdc_submit_update(fb_data);
+
+	mutex_unlock(&fb_data->queue_mutex);
+
+	return;
+}
+
+/*
+ * Sysfs functions
+ */
+static ssize_t show_update(struct device *device,
+				struct device_attribute *attr, char *buf) {
+	struct fb_info *info = dev_get_drvdata(device);
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+
+	return sprintf(buf, "mode%d\n", fb_data->fresh_param.wave_mode);
+}
+
+static ssize_t store_update(struct device *device,
+			 struct device_attribute *attr,
+			 const char *buf, size_t count)
+{
+	struct mxcfb_update_data update;
+	struct fb_info *info = dev_get_drvdata(device);
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	if (strncmp(buf, "direct", 6) == 0)
+		update.waveform_mode = fb_data->wv_modes.mode_du;
+	else if (strncmp(buf, "gc16", 4) == 0)
+		update.waveform_mode = fb_data->wv_modes.mode_gc16;
+	else if (strncmp(buf, "gc4", 3) == 0)
+		update.waveform_mode = fb_data->wv_modes.mode_gc4;
+	else if (strncmp(buf, "init", 4) == 0)
+		update.waveform_mode = fb_data->wv_modes.mode_init;
+	else if (strncmp(buf, "gu", 2) == 0)
+		update.waveform_mode = SPDC_WAV_MODE_3;
+	else if (strncmp(buf, "auto", 4) == 0)
+		update.waveform_mode = WAVEFORM_MODE_AUTO;
+
+	/* Now, request full screen update */
+	update.update_region.left = 0;
+	update.update_region.width = fb_data->spdc_fb_var.xres;
+	update.update_region.top = 0;
+	update.update_region.height = fb_data->spdc_fb_var.yres;
+	update.update_mode = UPDATE_MODE_FULL;
+	update.temp = SPDC_DEFAULT_TEMP;
+	update.update_marker = 0;
+	update.flags = 0;
+	dev_dbg(fb_data->dev, "rotation:%d, gray:%d\n",
+	fb_data->spdc_fb_var.rotate, fb_data->spdc_fb_var.grayscale);
+
+	mxc_spdc_fb_send_update(&update, info);
+
+	return count;
+}
+
+static ssize_t fresh_show(struct device *device, struct device_attribute *attr,
+			  char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(device);
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+
+	return sprintf(buf, "%d\n", fb_data->operation_mode);
+}
+
+static ssize_t fresh_store(struct device *device, struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct fb_info *info = dev_get_drvdata(device);
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	int ret, operation;
+
+	ret = kstrtoint(buf, 10, &operation);
+	if (ret)
+		return ret;
+
+	if (operation == SPDC_DEEP_REFRESH)
+		fb_data->is_deep_fresh = true;
+	fb_data->operation_mode = operation;
+	if (operation > SPDC_NO_OPERATION &&
+		operation < SPDC_FULL_REFRESH)
+		mxc_operaton_update(fb_data);
+	else
+		mxc_spdc_refresh_display(fb_data);
+
+	return count;
+}
+
+static ssize_t temp_show(struct device *device,
+		struct device_attribute *attr, char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(device);
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	int temp;
+
+	temp = fb_data->fresh_param.temper >> 1;
+	return sprintf(buf, "%d\n", temp);
+}
+
+static ssize_t initset_show(struct device *device,
+		struct device_attribute *attr, char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(device);
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	u32 init_val;
+
+	get_panel_init_set(&fb_data->panel_set, &init_val);
+	return sprintf(buf, "%x\n", init_val);
+}
+
+static ssize_t concurrency_show(struct device *device,
+		struct device_attribute *attr, char *buf)
+{
+	struct fb_info *info = dev_get_drvdata(device);
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	bool temp;
+
+	temp = (fb_data->fresh_param.concur ? 1 : 0);
+	return sprintf(buf, "%d\n", temp);
+}
+
+static ssize_t concurrency_update(struct device *device,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct fb_info *info = dev_get_drvdata(device);
+	mxc_spdc_t *fb_data = (mxc_spdc_t *)info;
+	int ret, concur;
+
+	ret = kstrtoint(buf, 10, &concur);
+	if (ret)
+		return ret;
+
+	if (fb_data->fresh_param.concur != concur) {
+		fb_data->fresh_param.concur = concur;
+		spdc_set_update_concurrency(fb_data);
+	}
+
+	return count;
+}
+
+static DEVICE_ATTR(store_update, 0644, show_update, store_update);
+static DEVICE_ATTR(fresh, 0644, fresh_show, fresh_store);
+static DEVICE_ATTR(temp, 0644, temp_show, NULL);
+static DEVICE_ATTR(initset, 0644, initset_show, NULL);
+static DEVICE_ATTR(concurrency, 0644, concurrency_show, concurrency_update);
+
+static struct attribute *spdc_attributes[] = {
+	&dev_attr_store_update.attr,
+	&dev_attr_fresh.attr,
+	&dev_attr_temp.attr,
+	&dev_attr_initset.attr,
+	&dev_attr_concurrency.attr,
+	NULL
+};
+
+static const struct attribute_group spdc_attr_group = {
+	.attrs = spdc_attributes,
+};
+
+static int __devinit mxc_spdc_fb_probe(struct platform_device *pdev)
+{
+	struct fb_info *info;
+	mxc_spdc_t *fb_data;
+	struct resource *res, *mem;
+	struct fb_videomode *vmode;
+	int xres_virt, yres_virt, buf_size;
+	int xres_virt_rot, yres_virt_rot, pix_size_rot;
+	struct fb_var_screeninfo *var_info;
+	struct fb_fix_screeninfo *fix_info;
+	struct pxp_config_data *pxp_conf;
+	struct pxp_proc_data *proc_data;
+	struct scatterlist *sg;
+	struct update_data_list *upd_list;
+	struct update_data_list *plist, *temp_list;
+	char *options, *opt;
+	char *panel_str = NULL;
+	char name[] = "mxcspdcfb";
+	unsigned long x_mem_size = 0;
+	u32 i, cmap_size;
+	int ret = 0;
+
+	fb_data = (mxc_spdc_t *)framebuffer_alloc(sizeof(mxc_spdc_t),
+							&pdev->dev);
+	if (!fb_data) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	info = &fb_data->info;
+	fb_data->dev = &pdev->dev;
+	platform_set_drvdata(pdev, fb_data);
+
+	fb_data->pdata = pdev->dev.platform_data;
+	if ((fb_data->pdata == NULL) || (fb_data->pdata->num_modes < 1)
+		|| (fb_data->pdata->spdc_mode == NULL)
+		|| (fb_data->pdata->spdc_mode->vmode == NULL)) {
+		ret = -EINVAL;
+		goto dealloc_fb;
+	}
+
+	/*get panel para from command*/
+	if (fb_get_options(name, &options)) {
+		ret = -ENODEV;
+		goto dealloc_fb;
+	}
+	if (options)
+		while ((opt = strsep(&options, ",")) != NULL) {
+			if (!*opt)
+				continue;
+			if (!strncmp(opt, "bpp=", 4)) {
+				if (kstrtoint(opt + 4, 0,
+				&fb_data->default_bpp) < 0)
+					fb_data->default_bpp = 0;
+			} else if (!strncmp(opt, "x_mem=", 6))
+				x_mem_size = memparse(opt + 6, NULL);
+			else
+				panel_str = opt;
+		}
+
+	if (!fb_data->default_bpp)
+		fb_data->default_bpp = SPDC_DEFAULT_BPP;
+
+	/* Set default (first defined mode) for a match */
+	mxc_spdc_find_match_mode(fb_data);
+
+	if (panel_str)
+		for (i = 0; i < fb_data->pdata->num_modes; i++)
+			if (!strcmp(fb_data->pdata->spdc_mode[i].vmode->name,
+							panel_str)) {
+				fb_data->cur_mode =
+					 &fb_data->pdata->spdc_mode[i];
+				break;
+			}
+	vmode = fb_data->cur_mode->vmode;
+
+	/* Allocate color map for the FB */
+	cmap_size = 256;
+	ret = fb_alloc_cmap(&info->cmap, cmap_size, 0);
+	if (ret)
+		goto dealloc_fb;
+
+	/*
+	 * GPU alignment restrictions dictate framebuffer parameters:
+	 * - 32-byte alignment for buffer width
+	 * - 128-byte alignment for buffer height
+	 * => 4K buffer alignment for buffer start
+	 */
+	xres_virt = ALIGN(vmode->xres, 32);
+	yres_virt = ALIGN(vmode->yres, 128);
+	fb_data->max_pix_size = PAGE_ALIGN(xres_virt * yres_virt);
+
+	/*
+	 * Have to check to see if aligned buffer size when rotated
+	 * is bigger than when not rotated, and use the max
+	 */
+	xres_virt_rot = ALIGN(vmode->yres, 32);
+	yres_virt_rot = ALIGN(vmode->xres, 128);
+	pix_size_rot = PAGE_ALIGN(xres_virt_rot * yres_virt_rot);
+	fb_data->max_pix_size = (fb_data->max_pix_size > pix_size_rot) ?
+				fb_data->max_pix_size : pix_size_rot;
+	buf_size = fb_data->max_pix_size * fb_data->default_bpp/8;
+
+	/* Compute the number of screens needed based on X memory requested */
+	if (x_mem_size > 0) {
+		fb_data->num_screens = DIV_ROUND_UP(x_mem_size, buf_size);
+		if (fb_data->num_screens < NUM_SCREENS_MIN)
+			fb_data->num_screens = NUM_SCREENS_MIN;
+		else if (buf_size * fb_data->num_screens > SZ_16M)
+			fb_data->num_screens = SZ_16M / buf_size;
+	} else
+		fb_data->num_screens = NUM_SCREENS_MIN;
+
+	fb_data->map_size = buf_size * fb_data->num_screens;
+	dev_dbg(&pdev->dev, "memory allocate: %d\n", fb_data->map_size);
+
+	/* get IO memory*/
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "failed to get memory register\n");
+		ret = -ENXIO;
+		goto release_cmap;
+	}
+
+	mem = request_mem_region(res->start, resource_size(res), pdev->name);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "failed to get memory region\n");
+		ret = -ENOENT;
+		goto release_cmap;
+	}
+
+	fb_data->hwp = ioremap(res->start, SZ_4K);
+	if (fb_data->hwp == NULL) {
+		dev_err(&pdev->dev, "ioremap registers failed\n");
+		ret = -ENOENT;
+		goto release_mem;
+	}
+
+	/* Allocate FB memory */
+	fb_data->virt_start = dma_alloc_writecombine(&pdev->dev,
+		fb_data->map_size, &fb_data->phys_start, GFP_DMA);
+	if (fb_data->virt_start == NULL) {
+		dev_err(&pdev->dev, "probe err - dma_alloc for framebuffer\n");
+		ret = -ENOMEM;
+		goto release_hwp;
+	}
+
+	var_info = &info->var;
+	var_info->activate = FB_ACTIVATE_TEST;
+	var_info->bits_per_pixel = fb_data->default_bpp;
+	var_info->xres = vmode->xres;
+	var_info->yres = vmode->yres;
+	var_info->xres_virtual = xres_virt;
+	var_info->yres_virtual = yres_virt * fb_data->num_screens;
+	var_info->pixclock = vmode->pixclock;
+	var_info->left_margin = vmode->left_margin;
+	var_info->right_margin = vmode->right_margin;
+	var_info->upper_margin = vmode->upper_margin;
+	var_info->lower_margin = vmode->lower_margin;
+	var_info->hsync_len = vmode->hsync_len;
+	var_info->vsync_len = vmode->vsync_len;
+	var_info->vmode = FB_VMODE_NONINTERLACED;
+
+	switch (fb_data->default_bpp) {
+	case 32:
+	case 24:
+		var_info->red.offset = 16;
+		var_info->red.length = 8;
+		var_info->green.offset = 8;
+		var_info->green.length = 8;
+		var_info->blue.offset = 0;
+		var_info->blue.length = 8;
+		break;
+	case 16:
+		var_info->red.offset = 11;
+		var_info->red.length = 5;
+		var_info->green.offset = 5;
+		var_info->green.length = 6;
+		var_info->blue.offset = 0;
+		var_info->blue.length = 5;
+		break;
+	case 8:
+		var_info->grayscale = GRAYSCALE_8BIT;
+
+		var_info->red.length = 8;
+		var_info->red.offset = 0;
+		var_info->red.msb_right = 0;
+		var_info->green.length = 8;
+		var_info->green.offset = 0;
+		var_info->green.msb_right = 0;
+		var_info->blue.length = 8;
+		var_info->blue.offset = 0;
+		var_info->blue.msb_right = 0;
+		break;
+	case 4:
+		var_info->grayscale = GRAYSCALE_4BIT;
+
+		var_info->red.length = 4;
+		var_info->red.offset = 0;
+		var_info->red.msb_right = 0;
+		var_info->green.length = 4;
+		var_info->green.offset = 0;
+		var_info->green.msb_right = 0;
+		var_info->blue.length = 4;
+		var_info->blue.offset = 0;
+		var_info->blue.msb_right = 0;
+		break;
+	default:
+		dev_err(&pdev->dev, "unsupported bit-width:%d\n",
+			fb_data->default_bpp);
+		ret = -EINVAL;
+		goto release_dma_fb;
+	}
+
+	fix_info = &info->fix;
+	strcpy(fix_info->id, SPDC_DRIVER_NAME);
+	fix_info->type = FB_TYPE_PACKED_PIXELS;
+	fix_info->visual = FB_VISUAL_TRUECOLOR;
+	fix_info->xpanstep = 0;
+	fix_info->ypanstep = 0;
+	fix_info->ywrapstep = 0;
+	fix_info->accel = FB_ACCEL_NONE;
+	fix_info->smem_start = fb_data->phys_start;
+	fix_info->smem_len = fb_data->map_size;
+	fix_info->ypanstep = 0;
+
+	/* Set up FB info */
+	fb_data->native_width = vmode->xres;
+	fb_data->native_height = vmode->yres;
+	info->screen_base = fb_data->virt_start;
+	info->screen_size = info->fix.smem_len;
+	info->fbops = &mxc_spdc_fb_ops;
+	info->var.activate = FB_ACTIVATE_NOW;
+	info->pseudo_palette = fb_data->pseudo_palette;
+	info->flags = FBINFO_FLAG_DEFAULT;
+
+	mxc_spdc_fb_set_fix(info);
+
+	/* use the same AXI and  PIX clock source */
+	fb_data->spdc_clk_axi = clk_get(fb_data->dev, "epdc_axi");
+	if (IS_ERR(fb_data->spdc_clk_axi)) {
+		dev_err(&pdev->dev, "Unable to get AXI clk."
+			"err = 0x%x\n", (int)fb_data->spdc_clk_axi);
+		ret = -ENODEV;
+		goto release_dma_fb;
+	}
+	fb_data->spdc_clk_pix = clk_get(fb_data->dev, "epdc_pix");
+	if (IS_ERR(fb_data->spdc_clk_pix)) {
+		dev_err(&pdev->dev, "Unable to get pix clk."
+			"err = 0x%x\n", (int)fb_data->spdc_clk_pix);
+		ret = -ENODEV;
+		goto release_dma_fb;
+	}
+
+	/*
+	 * Initialize update list and allocate buffer.
+	 */
+	INIT_LIST_HEAD(&fb_data->upd_pending_list);
+	INIT_LIST_HEAD(&fb_data->upd_buf_queue);
+	INIT_LIST_HEAD(&fb_data->upd_buf_free_list);
+	INIT_LIST_HEAD(&fb_data->upd_buf_preprocess_list);
+	INIT_LIST_HEAD(&fb_data->full_marker_list);
+
+	fb_data->max_num_updates = SPDC_MAX_NUM_UPDATES;
+	fb_data->max_num_buffers = SPDC_MAX_NUM_BUFFERS;
+
+	/* Allocate free buffers */
+	for (i = 0; i < fb_data->max_num_updates; i++) {
+		upd_list = kzalloc(sizeof(*upd_list), GFP_KERNEL);
+		if (upd_list == NULL) {
+			ret = -ENOMEM;
+			goto release_dma_fb;
+		}
+
+		/* Add newly allocated buffer to free list */
+		list_add(&upd_list->list, &fb_data->upd_buf_free_list);
+	}
+
+	/* Allocate PXP output buffer */
+	fb_data->virt_addr_updbuf =
+		kzalloc(sizeof(void *) * fb_data->max_num_buffers, GFP_KERNEL);
+	fb_data->phys_addr_updbuf =
+		kzalloc(sizeof(dma_addr_t) * fb_data->max_num_buffers,
+			GFP_KERNEL);
+	for (i = 0; i < fb_data->max_num_buffers; i++) {
+		/*
+		 * Allocate memory for PxP output buffer.
+		 * Output raw data is Y4 format.
+		 * Each update buffer is 1/2 byte per pixel, and can
+		 * be as big as the full-screen frame buffer
+		 */
+		fb_data->virt_addr_updbuf[i] =
+			dma_alloc_coherent(fb_data->info.device,
+				fb_data->max_pix_size / 2,
+				   &fb_data->phys_addr_updbuf[i], GFP_DMA);
+		if (fb_data->virt_addr_updbuf[i] == NULL) {
+			ret = -ENOMEM;
+			goto release_freebuf_lists;
+		}
+	}
+	/* Counter indicating which update buffer should be used next. */
+	fb_data->upd_buffer_num = 0;
+
+	/* Allocate memory for partical process region buffer.
+	 *  Output raw data is Y4 format.
+	 */
+	fb_data->virt_addr_copybuf =
+	dma_alloc_coherent(fb_data->info.device,
+			fb_data->max_pix_size / 2,
+			&fb_data->phys_addr_copybuf, GFP_DMA);
+	if (fb_data->virt_addr_copybuf == NULL) {
+		ret = -ENOMEM;
+		goto release_output_buf;
+	}
+
+	/* Allocate next & current & privious & count & lut buffers.
+	 *  next buffer size is Y4 raw data
+	 */
+	fb_data->next_buf_size =
+		(fb_data->map_size / fb_data->num_screens) >> 1;
+	fb_data->virt_next_buf =
+		dma_alloc_coherent(&pdev->dev, fb_data->next_buf_size,
+				&fb_data->phy_next_buf, GFP_DMA);
+	if (fb_data->virt_next_buf == NULL) {
+		dev_err(&pdev->dev, "Can't allocate mem for next buf!\n");
+		ret = -ENOMEM;
+		goto release_copy_buf;
+	}
+
+	fb_data->current_buf_size =
+		(fb_data->map_size / fb_data->num_screens) >> 1;
+	fb_data->virt_current_buf =
+		dma_alloc_coherent(&pdev->dev, fb_data->current_buf_size,
+				&fb_data->phy_current_buf, GFP_DMA);
+	if (fb_data->virt_current_buf == NULL) {
+		dev_err(&pdev->dev, "Can't allocate mem for current buf!\n");
+		ret = -ENOMEM;
+		goto release_next_buf;
+	}
+
+	fb_data->pre_buf_size =
+		(fb_data->map_size / fb_data->num_screens) >> 1;
+	fb_data->virt_pre_buf =
+		dma_alloc_coherent(&pdev->dev, fb_data->pre_buf_size,
+				&fb_data->phy_pre_buf, GFP_DMA);
+	if (fb_data->virt_pre_buf == NULL) {
+		dev_err(&pdev->dev, "Can't allocate mem for current buf!\n");
+		ret = -ENOMEM;
+		goto release_current_buf;
+	}
+
+	fb_data->cnt_buf_size = info->var.xres * info->var.yres;
+	fb_data->virt_cnt_buf =
+		dma_alloc_coherent(&pdev->dev, fb_data->cnt_buf_size,
+				&fb_data->phy_cnt_buf, GFP_DMA);
+	if (fb_data->virt_cnt_buf == NULL) {
+		dev_err(&pdev->dev, "Can't allocate mem for current buf!\n");
+		ret = -ENOMEM;
+		goto release_pre_buf;
+	}
+
+	fb_data->lut_buf_size = SZ_1M;
+	fb_data->virt_lut_buf =
+		dma_alloc_coherent(&pdev->dev, fb_data->lut_buf_size,
+				&fb_data->phy_lut_buf, GFP_DMA);
+	if (fb_data->virt_lut_buf == NULL) {
+		dev_err(&pdev->dev, "Can't allocate mem for current buf!\n");
+		ret = -ENOMEM;
+		goto release_cnt_buf;
+	}
+
+	/* Initialize SPDC pins */
+	if (fb_data->pdata->get_pins)
+		fb_data->pdata->get_pins();
+
+	fb_data->hw_ready = false;
+	fb_data->hw_initializing = false;
+
+	/*
+	 * Set default waveform mode values.
+	 * Should be overwritten via ioctl.
+	 */
+	fb_data->wv_modes.mode_init = SPDC_WAV_MODE_DEFAULT;
+	fb_data->wv_modes.mode_du = SPDC_WAV_MODE_4;
+	fb_data->wv_modes.mode_gc4 = SPDC_WAV_MODE_2;
+	fb_data->wv_modes.mode_gc8 = SPDC_WAV_MODE_1;
+	fb_data->wv_modes.mode_gc16 = SPDC_WAV_MODE_1;
+	fb_data->wv_modes.mode_gc32 = SPDC_WAV_MODE_1;
+
+	fb_data->auto_mode = AUTO_UPDATE_MODE_REGION_MODE;
+	fb_data->upd_scheme = UPDATE_SCHEME_QUEUE_AND_MERGE;
+	fb_data->spdc_fb_var = *var_info;
+	fb_data->fb_offset = 0;
+
+	fb_data->blank = FB_BLANK_UNBLANK;
+	fb_data->powering_down = false;
+	fb_data->power_state = POWER_STATE_OFF;
+	fb_data->pwrdown_delay = 0;
+	fb_data->cur_update = NULL;
+	fb_data->waiting_for_idle = false;
+	fb_data->order_cnt = 0;
+	fb_data->waiting_for_wb = false;
+	fb_data->wait_for_powerdown = false;
+	fb_data->updates_active = false;
+
+	spin_lock_init(&fb_data->lock);
+	mutex_init(&fb_data->queue_mutex);
+	mutex_init(&fb_data->pxp_mutex);
+	mutex_init(&fb_data->power_mutex);
+	init_completion(&fb_data->lut_down);
+	init_completion(&fb_data->init_finish);
+	init_completion(&fb_data->update_finish);
+	INIT_DELAYED_WORK(&fb_data->spdc_done_work, spdc_done_work_func);
+
+	fb_data->spdc_submit_workqueue = alloc_workqueue("SPDC Submit",
+					WQ_MEM_RECLAIM | WQ_HIGHPRI |
+					WQ_CPU_INTENSIVE | WQ_UNBOUND, 1);
+	INIT_WORK(&fb_data->spdc_submit_work, spdc_submit_work_func);
+	fb_data->spdc_intr_workqueue = alloc_workqueue("SPDC Interrupt",
+					WQ_MEM_RECLAIM | WQ_HIGHPRI |
+					WQ_CPU_INTENSIVE | WQ_UNBOUND, 1);
+	INIT_WORK(&fb_data->spdc_intr_work, spdc_intr_work_func);
+
+	/* Retrieve spdc IRQ num */
+	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (res == NULL) {
+		dev_err(&pdev->dev, "cannot get IRQ resource\n");
+		ret = -ENODEV;
+		goto release_lut_buf;
+	}
+	fb_data->spdc_irq = res->start;
+	ret = request_irq(fb_data->spdc_irq, mxc_spdc_irq_handler, 0,
+			"fb_dma", fb_data);
+	if (ret) {
+		dev_err(&pdev->dev, "request_irq (%d) failed with error %d\n",
+			fb_data->spdc_irq, ret);
+		ret = -ENODEV;
+		goto release_lut_buf;
+	}
+
+	/* define deferred io */
+	info->fbdefio = &mxc_spdc_fb_defio;
+#ifdef CONFIG_FB_MXC_SIPIX_AUTO_UPDATE_MODE
+	fb_deferred_io_init(info);
+#endif
+
+	/* get pmic regulators */
+	fb_data->display_regulator = regulator_get(NULL, "DISPLAY");
+	if (IS_ERR(fb_data->display_regulator)) {
+		dev_err(&pdev->dev, "Unable to get display PMIC regulator."
+			"err = 0x%x\n", (int)fb_data->display_regulator);
+		ret = -ENODEV;
+		goto release_irq;
+	}
+	fb_data->vcom_regulator = regulator_get(NULL, "VCOM");
+	if (IS_ERR(fb_data->vcom_regulator)) {
+		regulator_put(fb_data->display_regulator);
+		dev_err(&pdev->dev, "Unable to get VCOM regulator."
+			"err = 0x%x\n", (int)fb_data->vcom_regulator);
+		ret = -ENODEV;
+		goto release_regulator1;
+	}
+	fb_data->v3p3_regulator = regulator_get(NULL, "V3P3");
+	if (IS_ERR(fb_data->v3p3_regulator)) {
+		regulator_put(fb_data->vcom_regulator);
+		regulator_put(fb_data->display_regulator);
+		dev_err(&pdev->dev, "Unable to get V3P3 regulator."
+			"err = 0x%x\n", (int)fb_data->vcom_regulator);
+		ret = -ENODEV;
+		goto release_regulator2;
+	}
+
+	/*
+	 * Fill out PxP config data structure based on FB info and
+	 * processing tasks required
+	 */
+	pxp_conf = &fb_data->pxp_conf;
+	proc_data = &pxp_conf->proc_data;
+
+	/* Initialize non-channel-specific PxP parameters */
+	proc_data->drect.left = proc_data->srect.left = 0;
+	proc_data->drect.top = proc_data->srect.top = 0;
+	proc_data->drect.width = fb_data->info.var.xres;
+	proc_data->srect.width = fb_data->info.var.xres;
+	proc_data->drect.height = fb_data->info.var.yres;
+	proc_data->srect.height = fb_data->info.var.yres;
+	proc_data->scaling = 0;
+	proc_data->hflip = 0;
+	proc_data->vflip = 0;
+	proc_data->rotate = 0;
+	proc_data->bgcolor = 0;
+	proc_data->overlay_state = 0;
+	proc_data->lut_transform = PXP_LUT_NONE;
+	proc_data->lut_map = NULL;
+
+	/*
+	 * We initially configure PxP for RGB->YUV conversion,
+	 * and only write out Y component of the result.
+	 */
+
+	/*
+	 * Initialize S0 channel parameters
+	 * Parameters should match FB format/width/height
+	 */
+	pxp_conf->s0_param.pixel_fmt = PXP_PIX_FMT_RGB565;
+	pxp_conf->s0_param.width = fb_data->info.var.xres_virtual;
+	pxp_conf->s0_param.height = fb_data->info.var.yres;
+	pxp_conf->s0_param.color_key = -1;
+	pxp_conf->s0_param.color_key_enable = false;
+
+	/*
+	 * Initialize OL0 channel parameters
+	 * No overlay will be used for PxP operation
+	 */
+	for (i = 0; i < 8; i++) {
+		pxp_conf->ol_param[i].combine_enable = false;
+		pxp_conf->ol_param[i].width = 0;
+		pxp_conf->ol_param[i].height = 0;
+		pxp_conf->ol_param[i].pixel_fmt = PXP_PIX_FMT_RGB565;
+		pxp_conf->ol_param[i].color_key_enable = false;
+		pxp_conf->ol_param[i].color_key = -1;
+		pxp_conf->ol_param[i].global_alpha_enable = false;
+		pxp_conf->ol_param[i].global_alpha = 0;
+		pxp_conf->ol_param[i].local_alpha_enable = false;
+	}
+
+	/*
+	 * Initialize Output channel parameters
+	 * Output is Y-only greyscale
+	 * Output width/height will vary based on update region size
+	 */
+	pxp_conf->out_param.width = fb_data->info.var.xres;
+	pxp_conf->out_param.height = fb_data->info.var.yres;
+	pxp_conf->out_param.pixel_fmt = PXP_PIX_FMT_GY04;
+	pxp_conf->out_param.stride = pxp_conf->out_param.width;
+
+	/* Initialize color map for conversion of 8-bit gray pixels */
+	fb_data->pxp_conf.proc_data.lut_map = kmalloc(256, GFP_KERNEL);
+	if (fb_data->pxp_conf.proc_data.lut_map == NULL) {
+		dev_err(&pdev->dev, "Can't allocate mem for lut map!\n");
+		ret = -ENOMEM;
+		goto release_regulator3;
+	}
+	for (i = 0; i < 256; i++)
+		fb_data->pxp_conf.proc_data.lut_map[i] = i;
+
+	fb_data->pxp_conf.proc_data.lut_map_updated = true;
+
+	/*
+	 * Ensure this is set to NULL here...we will initialize pxp_chan
+	 * later in our thread.
+	 */
+	fb_data->pxp_chan = NULL;
+
+	/* Initialize Scatter-gather list containing 2 buffer addresses. */
+	sg = fb_data->sg;
+	sg_init_table(sg, 2);
+
+	/*
+	 * For use in PxP transfers:
+	 * sg[0] holds the FB buffer pointer
+	 * sg[1] holds the Output buffer pointer (configured before TX request)
+	 */
+	sg_dma_address(&sg[0]) = info->fix.smem_start;
+	sg_set_page(&sg[0], virt_to_page(info->screen_base),
+		info->fix.smem_len, offset_in_page(info->screen_base));
+
+	/* Register FB */
+	ret = register_framebuffer(info);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"register framebuffer failed\n");
+		goto release_lutmap;
+	}
+
+	ret = sysfs_create_group(&info->device->kobj, &spdc_attr_group);
+	if (ret)
+		dev_err(&pdev->dev, "Unable to create file from fb_attrs\n");
+
+	/* use for spdc test */
+	g_fb_data = fb_data;
+
+	/* hw init */
+	spdc_fb_dev_init(fb_data);
+
+	/*detect spdc epd disp & tcon version*/
+	get_spdc_version(fb_data);
+
+	goto out;
+
+release_lutmap:
+	kfree(fb_data->pxp_conf.proc_data.lut_map);
+release_regulator3:
+	regulator_put(fb_data->v3p3_regulator);
+release_regulator2:
+	regulator_put(fb_data->vcom_regulator);
+release_regulator1:
+	regulator_put(fb_data->display_regulator);
+release_irq:
+	free_irq(fb_data->spdc_irq, fb_data);
+release_lut_buf:
+	dma_free_coherent(&pdev->dev, fb_data->pre_buf_size,
+		fb_data->virt_lut_buf, fb_data->phy_lut_buf);
+release_cnt_buf:
+	dma_free_coherent(&pdev->dev, fb_data->cnt_buf_size,
+		fb_data->virt_cnt_buf, fb_data->phy_cnt_buf);
+release_pre_buf:
+	dma_free_coherent(&pdev->dev, fb_data->pre_buf_size,
+		fb_data->virt_pre_buf, fb_data->phy_pre_buf);
+release_current_buf:
+	dma_free_coherent(&pdev->dev, fb_data->current_buf_size,
+		fb_data->virt_current_buf, fb_data->phy_current_buf);
+release_next_buf:
+	dma_free_coherent(&pdev->dev, fb_data->next_buf_size,
+		fb_data->virt_next_buf, fb_data->phy_next_buf);
+release_copy_buf:
+	dma_free_writecombine(&pdev->dev, fb_data->max_pix_size / 2,
+		fb_data->virt_addr_copybuf, fb_data->phys_addr_copybuf);
+release_output_buf:
+	for (i = 0; i < fb_data->max_num_buffers; i++)
+		if (fb_data->virt_addr_updbuf[i] != NULL)
+			dma_free_writecombine(&pdev->dev,
+			fb_data->max_pix_size / 2, fb_data->virt_addr_updbuf[i],
+			fb_data->phys_addr_updbuf[i]);
+	if (fb_data->virt_addr_updbuf != NULL)
+		kfree(fb_data->virt_addr_updbuf);
+	if (fb_data->phys_addr_updbuf != NULL)
+		kfree(fb_data->phys_addr_updbuf);
+release_freebuf_lists:
+	list_for_each_entry_safe(plist, temp_list, &fb_data->upd_buf_free_list,
+			list) {
+		list_del(&plist->list);
+		kfree(plist);
+	}
+release_dma_fb:
+	dma_free_writecombine(&pdev->dev,
+	fb_data->map_size, fb_data->virt_start, fb_data->phys_start);
+release_hwp:
+	iounmap(fb_data->hwp);
+release_mem:
+	release_resource(mem);
+release_cmap:
+	fb_dealloc_cmap(&info->cmap);
+dealloc_fb:
+	framebuffer_release(info);
+out:
+	return ret;
+}
+
+static int mxc_spdc_fb_remove(struct platform_device *pdev)
+{
+	struct update_data_list *plist, *temp_list;
+	mxc_spdc_t *fb_data = platform_get_drvdata(pdev);
+	struct fb_info *info = &fb_data->info;
+	int i;
+
+	mxc_spdc_fb_blank(FB_BLANK_POWERDOWN, &fb_data->info);
+
+	flush_workqueue(fb_data->spdc_submit_workqueue);
+	destroy_workqueue(fb_data->spdc_submit_workqueue);
+
+	regulator_put(fb_data->display_regulator);
+	regulator_put(fb_data->vcom_regulator);
+	regulator_put(fb_data->v3p3_regulator);
+
+	for (i = 0; i < fb_data->max_num_buffers; i++)
+		if (fb_data->virt_addr_updbuf[i] != NULL)
+			dma_free_writecombine(&pdev->dev,
+				fb_data->max_pix_size,
+				fb_data->virt_addr_updbuf[i],
+				fb_data->phys_addr_updbuf[i]);
+	if (fb_data->virt_addr_updbuf != NULL)
+		kfree(fb_data->virt_addr_updbuf);
+	if (fb_data->phys_addr_updbuf != NULL)
+		kfree(fb_data->phys_addr_updbuf);
+
+	/* free output temporary buffer */
+	dma_free_writecombine(&pdev->dev, fb_data->max_pix_size / 2,
+		fb_data->virt_addr_copybuf, fb_data->phys_addr_copybuf);
+
+	list_for_each_entry_safe(plist, temp_list,
+			&fb_data->upd_buf_free_list,
+			list) {
+		list_del(&plist->list);
+		kfree(plist);
+	}
+
+#if defined(CONFIG_FB_MXC_SIPIX_AUTO_UPDATE_MODE)
+	fb_deferred_io_cleanup(&fb_data->info);
+#endif
+
+	/* free frame buffer */
+	dma_free_writecombine(&pdev->dev, fb_data->map_size,
+			info->screen_base, fb_data->phys_start);
+
+	/* free SPDC hw allocate buffer */
+	dma_free_coherent(&pdev->dev, fb_data->next_buf_size,
+		fb_data->virt_next_buf, fb_data->phy_next_buf);
+	dma_free_coherent(&pdev->dev, fb_data->current_buf_size,
+		fb_data->virt_current_buf, fb_data->phy_current_buf);
+	dma_free_coherent(&pdev->dev, fb_data->pre_buf_size,
+		fb_data->virt_pre_buf, fb_data->phy_pre_buf);
+	dma_free_coherent(&pdev->dev, fb_data->cnt_buf_size,
+		fb_data->virt_cnt_buf, fb_data->phy_cnt_buf);
+	dma_free_coherent(&pdev->dev, fb_data->pre_buf_size,
+		fb_data->virt_lut_buf, fb_data->phy_lut_buf);
+
+	sysfs_remove_group(&info->device->kobj, &spdc_attr_group);
+	unregister_framebuffer(info);
+
+	if (fb_data->pdata->put_pins)
+		fb_data->pdata->put_pins();
+
+	free_irq(fb_data->spdc_irq, fb_data);
+	iounmap(fb_data->hwp);
+
+	fb_dealloc_cmap(&info->cmap);
+
+	framebuffer_release(info);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int mxc_spdc_fb_suspend(struct platform_device *pdev,
+				pm_message_t state)
+{
+	mxc_spdc_t *data = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = mxc_spdc_fb_blank(FB_BLANK_POWERDOWN, &data->info);
+
+	return ret;
+}
+
+static int mxc_spdc_fb_resume(struct platform_device *pdev)
+{
+	mxc_spdc_t *data = platform_get_drvdata(pdev);
+
+	mxc_spdc_fb_blank(FB_BLANK_UNBLANK, &data->info);
+	return 0;
+}
+#else
+#define mxc_spdc_fb_suspend	NULL
+#define mxc_spdc_fb_resume	NULL
+#endif
+
+static struct platform_driver mxc_spdc_fb_driver = {
+	.probe = mxc_spdc_fb_probe,
+	.remove = mxc_spdc_fb_remove,
+	.suspend = mxc_spdc_fb_suspend,
+	.resume = mxc_spdc_fb_resume,
+	.driver = {
+		.name = SPDC_DRIVER_NAME,
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init mxc_spdc_fb_init(void)
+{
+	return platform_driver_register(&mxc_spdc_fb_driver);
+}
+late_initcall(mxc_spdc_fb_init);
+
+static void __exit mxc_spdc_fb_exit(void)
+{
+	platform_driver_unregister(&mxc_spdc_fb_driver);
+}
+module_exit(mxc_spdc_fb_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor, Inc.");
+MODULE_DESCRIPTION("MXC SPDC framebuffer driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("fb");
diff --git a/drivers/video/mxc/mxc_spdc_fb.h b/drivers/video/mxc/mxc_spdc_fb.h
new file mode 100644
index 000000000000..34373788e83e
--- /dev/null
+++ b/drivers/video/mxc/mxc_spdc_fb.h
@@ -0,0 +1,352 @@
+/*
+ * Copyright (C) 2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
+ *
+ */
+
+#ifndef __MXC_SPDC_FB_H__
+#define __MXC_SPDC_FB_H__
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/mm.h>
+#include <linux/irq.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/fb.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/dmaengine.h>
+#include <linux/pxp_dma.h>
+#include <linux/mxcfb.h>
+#include <linux/list.h>
+#include <linux/uaccess.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/regulator/driver.h>
+#include <linux/dmaengine.h>
+#include <linux/gpio.h>
+#include <linux/kthread.h>
+#include <mach/epdc.h>
+#include <mach/dma.h>
+#include <linux/platform_device.h>
+
+#include <linux/fsl_devices.h>
+#include <linux/mxcfb.h>
+
+/*************************************
+*Register addresses
+*************************************/
+#define SPDC_DISP_TRIGGER		(0x000)
+#define SPDC_UPDATA_X_Y			(0x004)
+#define SPDC_UPDATE_W_H			(0x008)
+#define SPDC_LUT_PARA_UPDATE		(0x00C)
+#define SPDC_OPERATE			(0x010)
+#define SPDC_PANEL_INIT_SET		(0x014)
+#define SPDC_TEMP_INFO			(0x018)
+#define SPDC_NEXT_BUF			(0x01C)
+#define SPDC_CURRENT_BUF		(0x020)
+#define SPDC_PRE_BUF			(0x024)
+#define SPDC_CNT_BUF			(0x028)
+#define SPDC_LUT_BUF			(0x02C)
+#define SPDC_INT_ENABLE			(0x030)
+#define SPDC_INT_STA_CLR		(0x034)
+#define SPDC_INIT_STA_CLR		(0x038)
+#define SPDC_STATUS			(0x03C)
+#define SPDC_DISP_VER			(0x040)
+#define SPDC_TCON_VER			(0x044)
+#define SPDC_SW_GATE_CLK		(0x048)
+
+/*
+ * Register field definitions
+ */
+enum {
+	SPDC_DISP_TRIGGER_ENABLE = 0x1,
+	SPDC_DISP_TRIGGER_FLASH = 0x10,
+
+	/*SPDC clock gate*/
+	SPDC_SW_GATE_CLK_ENABLE = 0x1,
+
+	/* waveform mode mask */
+	SPDC_WAV_MODE_MASK = (0x7 << 1),
+
+	/* SPDC interrupt IRQ mask define */
+	SPDC_IRQ_FRAME_UPDATE = (0x1 << 0),
+	SPDC_IRQ_TCON_INIT = (0x1 << 1),
+	SPDC_IRQ_LUT_DOWNLOAD = (0x1 << 2),
+	SPDC_IRQ_ERR = (0x1 << 3),
+	SPDC_IRQ_ALL_MASK = 0xF,
+
+	/* SPDC interrupt status */
+	SPDC_IRQ_STA_FRAME_UPDATE = (0x1 << 0),
+	SPDC_IRQ_STA_TCON_INIT = (0x1 << 1),
+	SPDC_IRQ_STA_LUT_DOWNLOAD = (0x1 << 2),
+	SPDC_IRQ_STA_ERR = (0x1 << 3),
+	SPDC_IRQ_STA_ALL_MASK = 0xF,
+
+	/* SPDC update coordinate angle */
+	SPDC_UPDATE_W_H_MAX_SIZE = ((0x1 << 12) - 1),
+	SPDC_UPDATE_X_Y_MAX_SIZE = ((0x1 << 12) - 1),
+
+	/* SPDC TCON status */
+	SPDC_PANEL_STAUTS_BUSY = 0x1,
+	SPDC_TCON_STATUS_IDLE = (0x4 << 4),
+
+	/* SPDC EPD Operation mode */
+	SPDC_NO_OPERATION = 0,
+	SPDC_DISP_REFRESH = 1,
+	SPDC_DEEP_REFRESH = 0x2,
+	SPDC_DISP_RESET   = 0x4,
+	SPDC_SW_TCON_RESET = 0x5,
+	SPDC_SW_TCON_RESET_SET = 0x80000000,
+	SPDC_FULL_REFRESH = 0x6,
+
+	/* SPDC Concurrency mechanism: ACC */
+	SPDC_LUT_MODE_OFFSET = 0xb3,
+	SPDC_LUT_ACC_MODE = 0x4,
+
+	/* SPDC waveform */
+	SPDC_WAVEFORM_LUT_OFFSET_ADDR = 0x7600,
+	SPDC_LUT_PARA_LENTH = 0x100,
+	SPDC_LUT_FROM_MEM = 1,
+	SPDC_LUT_TO_MEM = 0,
+
+	/* SPDC submit update status */
+	SPDC_CONCUR_UPD = 0x4,
+	SPDC_QUEUE_UPD = 0x8,
+	SPDC_CONCUR_QUEUE = 0xc,
+};
+
+#define	SPDC_DRIVER_NAME	"imx_spdc_fb"
+
+/**
+ * SPDC EPD waveform display trigger mode
+ */
+enum {
+	SPDC_WAV_MODE_0	= 0,
+	SPDC_WAV_MODE_1,
+	SPDC_WAV_MODE_2,
+	SPDC_WAV_MODE_3,
+	SPDC_WAV_MODE_4,
+	SPDC_WAV_MODE_5,
+	SPDC_WAV_MODE_DEFAULT = SPDC_WAV_MODE_0,
+};
+
+/**
+ * SPDC controller ip version
+ */
+struct mxc_epd_disp_version {
+	u8 epd_type;
+	u8 lut_ver;
+	u16 product_id;
+};
+
+struct mxc_spdc_version {
+	struct mxc_epd_disp_version disp_ver;
+	u8 tcon_ver;
+};
+
+struct spdc_buffer_addr {
+	u32 next_buf_phys_addr;
+	u32 cur_buf_phys_addr;
+	u32 pre_buf_phys_addr;
+	u32 frm_cnt_buf_phys_addr;
+	u32 lut_buf_phys_addr;
+};
+
+struct partial_refresh_param {
+	struct spdc_buffer_addr buf_addr;
+	struct mxcfb_rect update_region;
+	int temper;
+	u32 blocking;
+	u32 wave_mode;
+	u32 stride;
+	u32 flash; /* only for waveform mode7 */
+	u32 concur; /* Concurrency mechanism: ACC */
+};
+
+/**
+ * SPDC lut data
+ */
+struct mxc_spdc_lut_para {
+	u8 lut_data[SPDC_LUT_PARA_LENTH];
+	u8 lut_addr[SPDC_LUT_PARA_LENTH];
+	bool lut_para_update_sta;
+};
+
+#define PORTRAIT	"portrait"
+#define LANDSCAPE	"Landscape"
+#define RESERVED	"Reserved"
+struct mxc_spdc_resolution_map_para {
+	u16 resolution;
+	u16 res_x;
+	u16 res_y;
+	char res_name[12];
+};
+
+struct update_marker_data {
+	struct list_head full_list;
+	struct list_head upd_list;
+	u32 update_marker;
+	struct completion update_completion;
+	int lut_num;
+	bool collision_test;
+	bool waiting;
+};
+
+struct update_desc_list {
+	struct list_head list;
+	struct mxcfb_update_data upd_data;
+	u32 spdc_offs;
+	u32 spdc_stride;
+	struct list_head upd_marker_list;
+	u32 update_order;
+};
+
+/* This structure represents a list node containing both
+ * a memory region allocated as an output buffer for the PxP
+ * update processing task, and the update
+ * description (mode, region, etc.)
+ */
+struct update_data_list {
+	struct list_head list;
+	dma_addr_t phys_addr;/* Pointer to phys address of processed Y buf */
+	void *virt_addr;
+	struct update_desc_list *update_desc;
+	int lut_num;
+	u64 collision_mask; /* SPDC cannot support collision detect,
+			     * align with EPDC driver struct */
+};
+
+typedef struct mxc_spdc_fb_param {
+	struct fb_info info;
+	struct fb_var_screeninfo spdc_fb_var;
+
+	char fw_str[24];
+	u32 pseudo_palette[16];
+	struct mxc_spdc_version spdc_ver;
+	struct imx_spdc_fb_mode *cur_mode;
+	struct imx_spdc_fb_platform_data *pdata;
+	struct partial_refresh_param fresh_param;
+	int blank;
+	bool updates_active;
+
+	u32 fb_offset;
+	u32 max_pix_size;
+	ssize_t map_size;
+	int native_width;
+	int native_height;
+	int default_bpp;
+
+	dma_addr_t phys_start;
+	void *virt_start;
+	struct device *dev;
+
+	int num_screens;
+	u32 order_cnt;
+	int max_num_updates;
+	u32 auto_mode;
+	u32 upd_scheme;
+	u32 operation_mode;
+	bool is_deep_fresh;
+	struct list_head full_marker_list;
+	struct list_head upd_pending_list;
+	struct list_head upd_buf_queue;
+	struct list_head upd_buf_free_list;
+	struct list_head upd_buf_preprocess_list;
+	u32 upd_preprocess_num;
+	int submit_upd_sta;
+	struct update_data_list *cur_update;
+	struct mutex queue_mutex;
+	struct imx_spdc_panel_init_set panel_set;
+	struct mxc_spdc_lut_para lut_para;
+	struct mxcfb_waveform_modes wv_modes;
+
+	dma_addr_t phy_next_buf;
+	dma_addr_t phy_pre_buf;
+	dma_addr_t phy_current_buf;
+	dma_addr_t phy_cnt_buf;
+	dma_addr_t phy_lut_buf;
+	void *virt_next_buf;
+	void *virt_current_buf;
+	void *virt_pre_buf;
+	void *virt_cnt_buf;
+	void *virt_lut_buf;
+	u32 next_buf_size;
+	u32 current_buf_size;
+	u32 pre_buf_size;
+	u32 cnt_buf_size;
+	u32 lut_buf_size;
+
+	dma_addr_t *phys_addr_updbuf;
+	void **virt_addr_updbuf;
+	u32 upd_buffer_num;
+	u32 max_num_buffers;
+
+	/* copy the processed data to next buffer relative region */
+	dma_addr_t phys_addr_copybuf;
+	void *virt_addr_copybuf;
+
+	int spdc_irq;
+	char __iomem *hwp;
+	struct clk *spdc_clk_axi;
+	struct clk *spdc_clk_pix;
+	struct regulator *display_regulator;
+	struct regulator *vcom_regulator;
+	struct regulator *v3p3_regulator;
+
+	spinlock_t lock;
+	int power_state;
+	bool powering_down;
+	int pwrdown_delay;
+	int wait_for_powerdown;
+	struct completion powerdown_compl;
+	struct mutex power_mutex;
+	bool fw_default_load;
+	bool hw_ready;
+	bool hw_initializing;
+	bool waiting_for_idle;
+	bool waiting_for_wb;
+	struct completion update_res_free;
+	struct completion lut_down;
+	struct completion init_finish;
+	struct completion update_finish;
+	struct completion updates_done;
+	struct delayed_work spdc_done_work;
+	struct workqueue_struct *spdc_submit_workqueue;
+	struct work_struct spdc_submit_work;
+	struct workqueue_struct *spdc_intr_workqueue;
+	struct work_struct spdc_intr_work;
+
+	/* FB elements related to PxP DMA */
+	struct completion pxp_tx_cmpl;
+	struct pxp_channel *pxp_chan;
+	struct pxp_config_data pxp_conf;
+	struct dma_async_tx_descriptor *txd;
+	dma_cookie_t cookie;
+	struct scatterlist sg[2];
+	struct mutex pxp_mutex; /* protects access to PxP */
+} mxc_spdc_t;
+
+#endif
diff --git a/drivers/video/mxc/mxcfb_seiko_wvga.c b/drivers/video/mxc/mxcfb_seiko_wvga.c
index 0ba4d9ed064f..c96238d80cb2 100644
--- a/drivers/video/mxc/mxcfb_seiko_wvga.c
+++ b/drivers/video/mxc/mxcfb_seiko_wvga.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
  */
 
 /*
@@ -122,7 +122,7 @@ static struct notifier_block nb = {
 static int __devinit lcd_probe(struct platform_device *pdev)
 {
 	int i;
-	struct mxc_lcd_platform_data *plat = pdev->dev.platform_data;
+	struct fsl_mxc_lcd_platform_data *plat = pdev->dev.platform_data;
 
 	if (plat) {
 		if (plat->reset)