Merge branch 'master' of git://git.denx.de/u-boot-arm

31 files changed, 1948 insertions, 915 deletions

diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
new file mode 100644
index 0000000000..2c54793d20
--- /dev/null
+++ b/drivers/crypto/Makefile
@@ -0,0 +1,47 @@
+#
+# Copyright (c) 2013 Samsung Electronics Co., Ltd.
+# 	http://www.samsung.com
+#
+# See file CREDITS for list of people who contributed to this
+# project.
+#
+# 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 $(TOPDIR)/config.mk
+
+LIB	:= $(obj)libcrypto.o
+
+COBJS-$(CONFIG_EXYNOS_ACE_SHA)	+= ace_sha.o
+
+COBJS	:= $(COBJS-y)
+SRCS	:= $(COBJS:.o=.c)
+OBJS	:= $(addprefix $(obj),$(COBJS))
+
+all:	$(LIB)
+
+$(LIB):	$(obj).depend $(OBJS)
+	$(call cmd_link_o_target, $(OBJS))
+
+
+#########################################################################
+
+# defines $(obj).depend target
+include $(SRCTREE)/rules.mk
+
+sinclude $(obj).depend
+
+########################################################################
diff --git a/drivers/crypto/ace_sha.c b/drivers/crypto/ace_sha.c
new file mode 100644
index 0000000000..53ebb33006
--- /dev/null
+++ b/drivers/crypto/ace_sha.c
@@ -0,0 +1,126 @@
+/*
+ * Advanced Crypto Engine - SHA Firmware
+ * Copyright (c) 2012  Samsung Electronics
+ *
+ * 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 <common.h>
+#include <sha256.h>
+#include <sha1.h>
+#include <asm/errno.h>
+#include "ace_sha.h"
+
+/* SHA1 value for the message of zero length */
+static const unsigned char sha1_digest_emptymsg[SHA1_SUM_LEN] = {
+	0xDA, 0x39, 0xA3, 0xEE, 0x5E, 0x6B, 0x4B, 0x0D,
+	0x32, 0x55, 0xBF, 0xFF, 0x95, 0x60, 0x18, 0x90,
+	0xAF, 0xD8, 0x07, 0x09};
+
+/* SHA256 value for the message of zero length */
+static const unsigned char sha256_digest_emptymsg[SHA256_SUM_LEN] = {
+	0xE3, 0xB0, 0xC4, 0x42, 0x98, 0xFC, 0x1C, 0x14,
+	0x9A, 0xFB, 0xF4, 0xC8, 0x99, 0x6F, 0xB9, 0x24,
+	0x27, 0xAE, 0x41, 0xE4, 0x64, 0x9B, 0x93, 0x4C,
+	0xA4, 0x95, 0x99, 0x1B, 0x78, 0x52, 0xB8, 0x55};
+
+int ace_sha_hash_digest(const unsigned char *pbuf, unsigned int buf_len,
+			unsigned char *pout, unsigned int hash_type)
+{
+	unsigned int i, reg, len;
+	unsigned int *pdigest;
+	struct exynos_ace_sfr *ace_sha_reg =
+		(struct exynos_ace_sfr *)samsung_get_base_ace_sfr();
+
+	if (buf_len == 0) {
+		/* ACE H/W cannot compute hash value for empty string */
+		if (hash_type == ACE_SHA_TYPE_SHA1)
+			memcpy(pout, sha1_digest_emptymsg, SHA1_SUM_LEN);
+		else
+			memcpy(pout, sha256_digest_emptymsg, SHA256_SUM_LEN);
+		return 0;
+	}
+
+	/* Flush HRDMA */
+	writel(ACE_FC_HRDMACFLUSH_ON, &ace_sha_reg->fc_hrdmac);
+	writel(ACE_FC_HRDMACFLUSH_OFF, &ace_sha_reg->fc_hrdmac);
+
+	/* Set byte swap of data in */
+	writel(ACE_HASH_SWAPDI_ON | ACE_HASH_SWAPDO_ON | ACE_HASH_SWAPIV_ON,
+	       &ace_sha_reg->hash_byteswap);
+
+	/* Select Hash input mux as external source */
+	reg = readl(&ace_sha_reg->fc_fifoctrl);
+	reg = (reg & ~ACE_FC_SELHASH_MASK) | ACE_FC_SELHASH_EXOUT;
+	writel(reg, &ace_sha_reg->fc_fifoctrl);
+
+	/* Set Hash as SHA1 or SHA256 and start Hash engine */
+	reg = (hash_type == ACE_SHA_TYPE_SHA1) ?
+		ACE_HASH_ENGSEL_SHA1HASH : ACE_HASH_ENGSEL_SHA256HASH;
+	reg |= ACE_HASH_STARTBIT_ON;
+	writel(reg, &ace_sha_reg->hash_control);
+
+	/* Enable FIFO mode */
+	writel(ACE_HASH_FIFO_ON, &ace_sha_reg->hash_fifo_mode);
+
+	/* Set message length */
+	writel(buf_len, &ace_sha_reg->hash_msgsize_low);
+	writel(0, &ace_sha_reg->hash_msgsize_high);
+
+	/* Set HRDMA */
+	writel((unsigned int)pbuf, &ace_sha_reg->fc_hrdmas);
+	writel(buf_len, &ace_sha_reg->fc_hrdmal);
+
+	while ((readl(&ace_sha_reg->hash_status) & ACE_HASH_MSGDONE_MASK) ==
+		ACE_HASH_MSGDONE_OFF) {
+		/*
+		 * PRNG error bit goes HIGH if a PRNG request occurs without
+		 * a complete seed setup. We are using this bit to check h/w
+		 * fault because proper setup is not expected in that case.
+		 */
+		if ((readl(&ace_sha_reg->hash_status)
+			& ACE_HASH_PRNGERROR_MASK) == ACE_HASH_PRNGERROR_ON)
+			return -EBUSY;
+	}
+
+	/* Clear MSG_DONE bit */
+	writel(ACE_HASH_MSGDONE_ON, &ace_sha_reg->hash_status);
+
+	/* Read hash result */
+	pdigest = (unsigned int *)pout;
+	len = (hash_type == ACE_SHA_TYPE_SHA1) ? SHA1_SUM_LEN : SHA256_SUM_LEN;
+
+	for (i = 0; i < len / 4; i++)
+		pdigest[i] = readl(&ace_sha_reg->hash_result[i]);
+
+	/* Clear HRDMA pending bit */
+	writel(ACE_FC_HRDMA, &ace_sha_reg->fc_intpend);
+
+	return 0;
+}
+
+void hw_sha256(const unsigned char *pbuf, unsigned int buf_len,
+			unsigned char *pout, unsigned int chunk_size)
+{
+	if (ace_sha_hash_digest(pbuf, buf_len, pout, ACE_SHA_TYPE_SHA256))
+		debug("ACE was not setup properly or it is faulty\n");
+}
+
+void hw_sha1(const unsigned char *pbuf, unsigned int buf_len,
+			unsigned char *pout, unsigned int chunk_size)
+{
+	if (ace_sha_hash_digest(pbuf, buf_len, pout, ACE_SHA_TYPE_SHA1))
+		debug("ACE was not setup properly or it is faulty\n");
+}
diff --git a/drivers/crypto/ace_sha.h b/drivers/crypto/ace_sha.h
new file mode 100644
index 0000000000..0292a08f6d
--- /dev/null
+++ b/drivers/crypto/ace_sha.h
@@ -0,0 +1,325 @@
+/*
+ * Header file for Advanced Crypto Engine - SFR definitions
+ *
+ * Copyright (c) 2012  Samsung Electronics
+ *
+ * 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 __ACE_SHA_H
+#define __ACE_SHA_H
+
+struct exynos_ace_sfr {
+	unsigned int	fc_intstat;	/* base + 0 */
+	unsigned int	fc_intenset;
+	unsigned int	fc_intenclr;
+	unsigned int	fc_intpend;
+	unsigned int	fc_fifostat;
+	unsigned int	fc_fifoctrl;
+	unsigned int	fc_global;
+	unsigned int	res1;
+	unsigned int	fc_brdmas;
+	unsigned int	fc_brdmal;
+	unsigned int	fc_brdmac;
+	unsigned int	res2;
+	unsigned int	fc_btdmas;
+	unsigned int	fc_btdmal;
+	unsigned int	fc_btdmac;
+	unsigned int	res3;
+	unsigned int	fc_hrdmas;
+	unsigned int	fc_hrdmal;
+	unsigned int	fc_hrdmac;
+	unsigned int	res4;
+	unsigned int	fc_pkdmas;
+	unsigned int	fc_pkdmal;
+	unsigned int	fc_pkdmac;
+	unsigned int	fc_pkdmao;
+	unsigned char   res5[0x1a0];
+
+	unsigned int	aes_control;	/* base + 0x200 */
+	unsigned int	aes_status;
+	unsigned char	res6[0x8];
+	unsigned int	aes_in[4];
+	unsigned int	aes_out[4];
+	unsigned int	aes_iv[4];
+	unsigned int	aes_cnt[4];
+	unsigned char   res7[0x30];
+	unsigned int	aes_key[8];
+	unsigned char   res8[0x60];
+
+	unsigned int	tdes_control;	/* base + 0x300 */
+	unsigned int	tdes_status;
+	unsigned char   res9[0x8];
+	unsigned int	tdes_key[6];
+	unsigned int	tdes_iv[2];
+	unsigned int	tdes_in[2];
+	unsigned int	tdes_out[2];
+	unsigned char   res10[0xc0];
+
+	unsigned int	hash_control;	/* base + 0x400 */
+	unsigned int	hash_control2;
+	unsigned int	hash_fifo_mode;
+	unsigned int	hash_byteswap;
+	unsigned int	hash_status;
+	unsigned char   res11[0xc];
+	unsigned int	hash_msgsize_low;
+	unsigned int	hash_msgsize_high;
+	unsigned int	hash_prelen_low;
+	unsigned int	hash_prelen_high;
+	unsigned int	hash_in[16];
+	unsigned int	hash_key_in[16];
+	unsigned int	hash_iv[8];
+	unsigned char   res12[0x30];
+	unsigned int	hash_result[8];
+	unsigned char   res13[0x20];
+	unsigned int	hash_seed[8];
+	unsigned int	hash_prng[8];
+	unsigned char   res14[0x180];
+
+	unsigned int	pka_sfr[5];		/* base + 0x700 */
+};
+
+/* ACE_FC_INT */
+#define ACE_FC_PKDMA			(1 << 0)
+#define ACE_FC_HRDMA			(1 << 1)
+#define ACE_FC_BTDMA			(1 << 2)
+#define ACE_FC_BRDMA			(1 << 3)
+#define ACE_FC_PRNG_ERROR		(1 << 4)
+#define ACE_FC_MSG_DONE		(1 << 5)
+#define ACE_FC_PRNG_DONE		(1 << 6)
+#define ACE_FC_PARTIAL_DONE		(1 << 7)
+
+/* ACE_FC_FIFOSTAT */
+#define ACE_FC_PKFIFO_EMPTY		(1 << 0)
+#define ACE_FC_PKFIFO_FULL		(1 << 1)
+#define ACE_FC_HRFIFO_EMPTY		(1 << 2)
+#define ACE_FC_HRFIFO_FULL		(1 << 3)
+#define ACE_FC_BTFIFO_EMPTY		(1 << 4)
+#define ACE_FC_BTFIFO_FULL		(1 << 5)
+#define ACE_FC_BRFIFO_EMPTY		(1 << 6)
+#define ACE_FC_BRFIFO_FULL		(1 << 7)
+
+/* ACE_FC_FIFOCTRL */
+#define ACE_FC_SELHASH_MASK		(3 << 0)
+#define ACE_FC_SELHASH_EXOUT		(0 << 0)	/* independent source */
+#define ACE_FC_SELHASH_BCIN		(1 << 0)	/* blk cipher input */
+#define ACE_FC_SELHASH_BCOUT		(2 << 0)	/* blk cipher output */
+#define ACE_FC_SELBC_MASK		(1 << 2)
+#define ACE_FC_SELBC_AES		(0 << 2)
+#define ACE_FC_SELBC_DES		(1 << 2)
+
+/* ACE_FC_GLOBAL */
+#define ACE_FC_SSS_RESET		(1 << 0)
+#define ACE_FC_DMA_RESET		(1 << 1)
+#define ACE_FC_AES_RESET		(1 << 2)
+#define ACE_FC_DES_RESET		(1 << 3)
+#define ACE_FC_HASH_RESET		(1 << 4)
+#define ACE_FC_AXI_ENDIAN_MASK		(3 << 6)
+#define ACE_FC_AXI_ENDIAN_LE		(0 << 6)
+#define ACE_FC_AXI_ENDIAN_BIBE		(1 << 6)
+#define ACE_FC_AXI_ENDIAN_WIBE		(2 << 6)
+
+/* Feed control - BRDMA control */
+#define ACE_FC_BRDMACFLUSH_OFF		(0 << 0)
+#define ACE_FC_BRDMACFLUSH_ON		(1 << 0)
+#define ACE_FC_BRDMACSWAP_ON		(1 << 1)
+#define ACE_FC_BRDMACARPROT_MASK	(0x7 << 2)
+#define ACE_FC_BRDMACARPROT_OFS	2
+#define ACE_FC_BRDMACARCACHE_MASK	(0xf << 5)
+#define ACE_FC_BRDMACARCACHE_OFS	5
+
+/* Feed control - BTDMA control */
+#define ACE_FC_BTDMACFLUSH_OFF		(0 << 0)
+#define ACE_FC_BTDMACFLUSH_ON		(1 << 0)
+#define ACE_FC_BTDMACSWAP_ON		(1 << 1)
+#define ACE_FC_BTDMACAWPROT_MASK	(0x7 << 2)
+#define ACE_FC_BTDMACAWPROT_OFS	2
+#define ACE_FC_BTDMACAWCACHE_MASK	(0xf << 5)
+#define ACE_FC_BTDMACAWCACHE_OFS	5
+
+/* Feed control - HRDMA control */
+#define ACE_FC_HRDMACFLUSH_OFF		(0 << 0)
+#define ACE_FC_HRDMACFLUSH_ON		(1 << 0)
+#define ACE_FC_HRDMACSWAP_ON		(1 << 1)
+#define ACE_FC_HRDMACARPROT_MASK	(0x7 << 2)
+#define ACE_FC_HRDMACARPROT_OFS	2
+#define ACE_FC_HRDMACARCACHE_MASK	(0xf << 5)
+#define ACE_FC_HRDMACARCACHE_OFS	5
+
+/* Feed control - PKDMA control */
+#define ACE_FC_PKDMACBYTESWAP_ON	(1 << 3)
+#define ACE_FC_PKDMACDESEND_ON		(1 << 2)
+#define ACE_FC_PKDMACTRANSMIT_ON	(1 << 1)
+#define ACE_FC_PKDMACFLUSH_ON		(1 << 0)
+
+/* Feed control - PKDMA offset */
+#define ACE_FC_SRAMOFFSET_MASK		0xfff
+
+/* AES control */
+#define ACE_AES_MODE_MASK		(1 << 0)
+#define ACE_AES_MODE_ENC		(0 << 0)
+#define ACE_AES_MODE_DEC		(1 << 0)
+#define ACE_AES_OPERMODE_MASK		(3 << 1)
+#define ACE_AES_OPERMODE_ECB		(0 << 1)
+#define ACE_AES_OPERMODE_CBC		(1 << 1)
+#define ACE_AES_OPERMODE_CTR		(2 << 1)
+#define ACE_AES_FIFO_MASK		(1 << 3)
+#define ACE_AES_FIFO_OFF		(0 << 3)	/* CPU mode */
+#define ACE_AES_FIFO_ON		(1 << 3)	/* FIFO mode */
+#define ACE_AES_KEYSIZE_MASK		(3 << 4)
+#define ACE_AES_KEYSIZE_128		(0 << 4)
+#define ACE_AES_KEYSIZE_192		(1 << 4)
+#define ACE_AES_KEYSIZE_256		(2 << 4)
+#define ACE_AES_KEYCNGMODE_MASK	(1 << 6)
+#define ACE_AES_KEYCNGMODE_OFF		(0 << 6)
+#define ACE_AES_KEYCNGMODE_ON		(1 << 6)
+#define ACE_AES_SWAP_MASK		(0x1f << 7)
+#define ACE_AES_SWAPKEY_OFF		(0 << 7)
+#define ACE_AES_SWAPKEY_ON		(1 << 7)
+#define ACE_AES_SWAPCNT_OFF		(0 << 8)
+#define ACE_AES_SWAPCNT_ON		(1 << 8)
+#define ACE_AES_SWAPIV_OFF		(0 << 9)
+#define ACE_AES_SWAPIV_ON		(1 << 9)
+#define ACE_AES_SWAPDO_OFF		(0 << 10)
+#define ACE_AES_SWAPDO_ON		(1 << 10)
+#define ACE_AES_SWAPDI_OFF		(0 << 11)
+#define ACE_AES_SWAPDI_ON		(1 << 11)
+#define ACE_AES_COUNTERSIZE_MASK	(3 << 12)
+#define ACE_AES_COUNTERSIZE_128	(0 << 12)
+#define ACE_AES_COUNTERSIZE_64		(1 << 12)
+#define ACE_AES_COUNTERSIZE_32		(2 << 12)
+#define ACE_AES_COUNTERSIZE_16		(3 << 12)
+
+/* AES status */
+#define ACE_AES_OUTRDY_MASK		(1 << 0)
+#define ACE_AES_OUTRDY_OFF		(0 << 0)
+#define ACE_AES_OUTRDY_ON		(1 << 0)
+#define ACE_AES_INRDY_MASK		(1 << 1)
+#define ACE_AES_INRDY_OFF		(0 << 1)
+#define ACE_AES_INRDY_ON		(1 << 1)
+#define ACE_AES_BUSY_MASK		(1 << 2)
+#define ACE_AES_BUSY_OFF		(0 << 2)
+#define ACE_AES_BUSY_ON		(1 << 2)
+
+/* TDES control */
+#define ACE_TDES_MODE_MASK		(1 << 0)
+#define ACE_TDES_MODE_ENC		(0 << 0)
+#define ACE_TDES_MODE_DEC		(1 << 0)
+#define ACE_TDES_OPERMODE_MASK		(1 << 1)
+#define ACE_TDES_OPERMODE_ECB		(0 << 1)
+#define ACE_TDES_OPERMODE_CBC		(1 << 1)
+#define ACE_TDES_SEL_MASK		(3 << 3)
+#define ACE_TDES_SEL_DES		(0 << 3)
+#define ACE_TDES_SEL_TDESEDE		(1 << 3)	/* TDES EDE mode */
+#define ACE_TDES_SEL_TDESEEE		(3 << 3)	/* TDES EEE mode */
+#define ACE_TDES_FIFO_MASK		(1 << 5)
+#define ACE_TDES_FIFO_OFF		(0 << 5)	/* CPU mode */
+#define ACE_TDES_FIFO_ON		(1 << 5)	/* FIFO mode */
+#define ACE_TDES_SWAP_MASK		(0xf << 6)
+#define ACE_TDES_SWAPKEY_OFF		(0 << 6)
+#define ACE_TDES_SWAPKEY_ON		(1 << 6)
+#define ACE_TDES_SWAPIV_OFF		(0 << 7)
+#define ACE_TDES_SWAPIV_ON		(1 << 7)
+#define ACE_TDES_SWAPDO_OFF		(0 << 8)
+#define ACE_TDES_SWAPDO_ON		(1 << 8)
+#define ACE_TDES_SWAPDI_OFF		(0 << 9)
+#define ACE_TDES_SWAPDI_ON		(1 << 9)
+
+/* TDES status */
+#define ACE_TDES_OUTRDY_MASK		(1 << 0)
+#define ACE_TDES_OUTRDY_OFF		(0 << 0)
+#define ACE_TDES_OUTRDY_ON		(1 << 0)
+#define ACE_TDES_INRDY_MASK		(1 << 1)
+#define ACE_TDES_INRDY_OFF		(0 << 1)
+#define ACE_TDES_INRDY_ON		(1 << 1)
+#define ACE_TDES_BUSY_MASK		(1 << 2)
+#define ACE_TDES_BUSY_OFF		(0 << 2)
+#define ACE_TDES_BUSY_ON		(1 << 2)
+
+/* Hash control */
+#define ACE_HASH_ENGSEL_MASK		(0xf << 0)
+#define ACE_HASH_ENGSEL_SHA1HASH	(0x0 << 0)
+#define ACE_HASH_ENGSEL_SHA1HMAC	(0x1 << 0)
+#define ACE_HASH_ENGSEL_SHA1HMACIN	(0x1 << 0)
+#define ACE_HASH_ENGSEL_SHA1HMACOUT	(0x9 << 0)
+#define ACE_HASH_ENGSEL_MD5HASH	(0x2 << 0)
+#define ACE_HASH_ENGSEL_MD5HMAC	(0x3 << 0)
+#define ACE_HASH_ENGSEL_MD5HMACIN	(0x3 << 0)
+#define ACE_HASH_ENGSEL_MD5HMACOUT	(0xb << 0)
+#define ACE_HASH_ENGSEL_SHA256HASH	(0x4 << 0)
+#define ACE_HASH_ENGSEL_SHA256HMAC	(0x5 << 0)
+#define ACE_HASH_ENGSEL_PRNG		(0x8 << 0)
+#define ACE_HASH_STARTBIT_ON		(1 << 4)
+#define ACE_HASH_USERIV_EN		(1 << 5)
+#define ACE_HASH_PAUSE_ON		(1 << 0)
+
+/* Hash control - FIFO mode */
+#define ACE_HASH_FIFO_MASK		(1 << 0)
+#define ACE_HASH_FIFO_OFF		(0 << 0)
+#define ACE_HASH_FIFO_ON		(1 << 0)
+
+/* Hash control - byte swap */
+#define ACE_HASH_SWAP_MASK		(0xf << 0)
+#define ACE_HASH_SWAPKEY_OFF		(0 << 0)
+#define	ACE_HASH_SWAPKEY_ON	(1 << 0)
+#define ACE_HASH_SWAPIV_OFF		(0 << 1)
+#define	ACE_HASH_SWAPIV_ON	(1 << 1)
+#define ACE_HASH_SWAPDO_OFF		(0 << 2)
+#define ACE_HASH_SWAPDO_ON		(1 << 2)
+#define ACE_HASH_SWAPDI_OFF		(0 << 3)
+#define ACE_HASH_SWAPDI_ON		(1 << 3)
+
+/* Hash status */
+#define ACE_HASH_BUFRDY_MASK		(1 << 0)
+#define ACE_HASH_BUFRDY_OFF		(0 << 0)
+#define ACE_HASH_BUFRDY_ON		(1 << 0)
+#define ACE_HASH_SEEDSETTING_MASK	(1 << 1)
+#define ACE_HASH_SEEDSETTING_OFF	(0 << 1)
+#define ACE_HASH_SEEDSETTING_ON	(1 << 1)
+#define ACE_HASH_PRNGBUSY_MASK		(1 << 2)
+#define ACE_HASH_PRNGBUSY_OFF		(0 << 2)
+#define ACE_HASH_PRNGBUSY_ON		(1 << 2)
+#define ACE_HASH_PARTIALDONE_MASK	(1 << 4)
+#define ACE_HASH_PARTIALDONE_OFF	(0 << 4)
+#define ACE_HASH_PARTIALDONE_ON	(1 << 4)
+#define ACE_HASH_PRNGDONE_MASK		(1 << 5)
+#define ACE_HASH_PRNGDONE_OFF		(0 << 5)
+#define ACE_HASH_PRNGDONE_ON		(1 << 5)
+#define ACE_HASH_MSGDONE_MASK		(1 << 6)
+#define ACE_HASH_MSGDONE_OFF		(0 << 6)
+#define ACE_HASH_MSGDONE_ON		(1 << 6)
+#define ACE_HASH_PRNGERROR_MASK	(1 << 7)
+#define ACE_HASH_PRNGERROR_OFF		(0 << 7)
+#define ACE_HASH_PRNGERROR_ON		(1 << 7)
+
+#define ACE_SHA_TYPE_SHA1		1
+#define ACE_SHA_TYPE_SHA256		2
+
+/**
+ * Computes hash value of input pbuf using ACE
+ *
+ * @param in_addr	A pointer to the input buffer
+ * @param bufleni	Byte length of input buffer
+ * @param out_addr	A pointer to the output buffer. When complete
+ *			32 bytes are copied to pout[0]...pout[31]. Thus, a user
+ *			should allocate at least 32 bytes at pOut in advance.
+ * @param hash_type SHA1 or SHA256
+ *
+ * @return		0 on Success, -1 on Failure (Timeout)
+ */
+int ace_sha_hash_digest(const uchar * in_addr, uint buflen,
+			uchar * out_addr, uint hash_type);
+#endif
diff --git a/drivers/mmc/bcm2835_sdhci.c b/drivers/mmc/bcm2835_sdhci.c
index b0afc3c9ce..54cfabfb91 100644
--- a/drivers/mmc/bcm2835_sdhci.c
+++ b/drivers/mmc/bcm2835_sdhci.c
@@ -39,6 +39,7 @@
 #include <common.h>
 #include <malloc.h>
 #include <sdhci.h>
+#include <asm/arch/timer.h>
 
 /* 400KHz is max freq for card ID etc. Use that as min */
 #define MIN_FREQ 400000
@@ -67,11 +68,11 @@ static inline void bcm2835_sdhci_raw_writel(struct sdhci_host *host, u32 val,
 	 * (Which is just as well - otherwise we'd have to nobble the DMA engine
 	 * too)
 	 */
-	while (get_timer(bcm_host->last_write) < bcm_host->twoticks_delay)
+	while (get_timer_us(bcm_host->last_write) < bcm_host->twoticks_delay)
 		;
 
 	writel(val, host->ioaddr + reg);
-	bcm_host->last_write = get_timer(0);
+	bcm_host->last_write = get_timer_us(0);
 }
 
 static inline u32 bcm2835_sdhci_raw_readl(struct sdhci_host *host, int reg)
diff --git a/drivers/mmc/fsl_esdhc.c b/drivers/mmc/fsl_esdhc.c
index 54b5363169..e945c0a470 100644
--- a/drivers/mmc/fsl_esdhc.c
+++ b/drivers/mmc/fsl_esdhc.c
@@ -327,9 +327,6 @@ esdhc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 	while (!(esdhc_read32(&regs->irqstat) & (IRQSTAT_CC | IRQSTAT_CTOE)))
 		;
 
-	if (data && (data->flags & MMC_DATA_READ))
-		check_and_invalidate_dcache_range(cmd, data);
-
 	irqstat = esdhc_read32(&regs->irqstat);
 	esdhc_write32(&regs->irqstat, irqstat);
 
@@ -400,9 +397,10 @@ esdhc_send_cmd(struct mmc *mmc, struct mmc_cmd *cmd, struct mmc_data *data)
 
 			if (irqstat & DATA_ERR)
 				return COMM_ERR;
-		} while (!(irqstat & IRQSTAT_TC) &&
-				(esdhc_read32(&regs->prsstat) & PRSSTAT_DLA));
+		} while ((irqstat & DATA_COMPLETE) != DATA_COMPLETE);
 #endif
+		if (data->flags & MMC_DATA_READ)
+			check_and_invalidate_dcache_range(cmd, data);
 	}
 
 	esdhc_write32(&regs->irqstat, -1);
@@ -580,6 +578,13 @@ int fsl_esdhc_initialize(bd_t *bis, struct fsl_esdhc_cfg *cfg)
 
 	mmc->host_caps = MMC_MODE_4BIT | MMC_MODE_8BIT | MMC_MODE_HC;
 
+	if (cfg->max_bus_width > 0) {
+		if (cfg->max_bus_width < 8)
+			mmc->host_caps &= ~MMC_MODE_8BIT;
+		if (cfg->max_bus_width < 4)
+			mmc->host_caps &= ~MMC_MODE_4BIT;
+	}
+
 	if (caps & ESDHC_HOSTCAPBLT_HSS)
 		mmc->host_caps |= MMC_MODE_HS_52MHz | MMC_MODE_HS;
 
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index c77c0c4f0f..35769c5ea3 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -73,7 +73,6 @@ COBJS-$(CONFIG_NAND_MXS) += mxs_nand.o
 COBJS-$(CONFIG_NAND_NDFC) += ndfc.o
 COBJS-$(CONFIG_NAND_NOMADIK) += nomadik.o
 COBJS-$(CONFIG_NAND_S3C2410) += s3c2410_nand.o
-COBJS-$(CONFIG_NAND_S3C64XX) += s3c64xx.o
 COBJS-$(CONFIG_NAND_SPEAR) += spr_nand.o
 COBJS-$(CONFIG_TEGRA_NAND) += tegra_nand.o
 COBJS-$(CONFIG_NAND_OMAP_GPMC) += omap_gpmc.o
@@ -82,6 +81,7 @@ COBJS-$(CONFIG_NAND_PLAT) += nand_plat.o
 else  # minimal SPL drivers
 
 COBJS-$(CONFIG_NAND_FSL_ELBC) += fsl_elbc_spl.o
+COBJS-$(CONFIG_NAND_MXC) += mxc_nand_spl.o
 
 endif # drivers
 endif # nand
diff --git a/drivers/mtd/nand/kirkwood_nand.c b/drivers/mtd/nand/kirkwood_nand.c
index bdab5aa795..0a99a10de1 100644
--- a/drivers/mtd/nand/kirkwood_nand.c
+++ b/drivers/mtd/nand/kirkwood_nand.c
@@ -74,7 +74,11 @@ void kw_nand_select_chip(struct mtd_info *mtd, int chip)
 int board_nand_init(struct nand_chip *nand)
 {
 	nand->options = NAND_COPYBACK | NAND_CACHEPRG | NAND_NO_PADDING;
+#if defined(CONFIG_NAND_ECC_BCH)
+	nand->ecc.mode = NAND_ECC_SOFT_BCH;
+#else
 	nand->ecc.mode = NAND_ECC_SOFT;
+#endif
 	nand->cmd_ctrl = kw_nand_hwcontrol;
 	nand->chip_delay = 40;
 	nand->select_chip = kw_nand_select_chip;
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index 04836c0062..eeba521942 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -22,10 +22,11 @@
 #include <nand.h>
 #include <linux/err.h>
 #include <asm/io.h>
-#if defined(CONFIG_MX25) || defined(CONFIG_MX27) || defined(CONFIG_MX35)
+#if defined(CONFIG_MX25) || defined(CONFIG_MX27) || defined(CONFIG_MX35) || \
+	defined(CONFIG_MX51) || defined(CONFIG_MX53)
 #include <asm/arch/imx-regs.h>
 #endif
-#include <fsl_nfc.h>
+#include "mxc_nand.h"
 
 #define DRIVER_NAME "mxc_nand"
 
@@ -33,7 +34,10 @@ struct mxc_nand_host {
 	struct mtd_info			mtd;
 	struct nand_chip		*nand;
 
-	struct fsl_nfc_regs __iomem	*regs;
+	struct mxc_nand_regs __iomem	*regs;
+#ifdef MXC_NFC_V3_2
+	struct mxc_nand_ip_regs __iomem	*ip_regs;
+#endif
 	int				spare_only;
 	int				status_request;
 	int				pagesize_2k;
@@ -75,7 +79,7 @@ static struct nand_ecclayout nand_hw_eccoob2k = {
 	.oobfree = { {2, 4}, {11, 11}, {27, 11}, {43, 11}, {59, 5} },
 };
 #endif
-#elif defined(MXC_NFC_V2_1)
+#elif defined(MXC_NFC_V2_1) || defined(MXC_NFC_V3_2)
 #ifndef CONFIG_SYS_NAND_LARGEPAGE
 static struct nand_ecclayout nand_hw_eccoob = {
 	.eccbytes = 9,
@@ -96,45 +100,14 @@ static struct nand_ecclayout nand_hw_eccoob2k = {
 #endif
 #endif
 
-#ifdef CONFIG_MX27
 static int is_16bit_nand(void)
 {
-	struct system_control_regs *sc_regs =
-		(struct system_control_regs *)IMX_SYSTEM_CTL_BASE;
-
-	if (readl(&sc_regs->fmcr) & NF_16BIT_SEL)
-		return 1;
-	else
-		return 0;
-}
-#elif defined(CONFIG_MX31)
-static int is_16bit_nand(void)
-{
-	struct clock_control_regs *sc_regs =
-		(struct clock_control_regs *)CCM_BASE;
-
-	if (readl(&sc_regs->rcsr) & CCM_RCSR_NF16B)
-		return 1;
-	else
-		return 0;
-}
-#elif defined(CONFIG_MX25) || defined(CONFIG_MX35)
-static int is_16bit_nand(void)
-{
-	struct ccm_regs *ccm = (struct ccm_regs *)IMX_CCM_BASE;
-
-	if (readl(&ccm->rcsr) & CCM_RCSR_NF_16BIT_SEL)
-		return 1;
-	else
-		return 0;
-}
+#if defined(CONFIG_SYS_NAND_BUSWIDTH_16BIT)
+	return 1;
 #else
-#warning "8/16 bit NAND autodetection not supported"
-static int is_16bit_nand(void)
-{
 	return 0;
-}
 #endif
+}
 
 static uint32_t *mxc_nand_memcpy32(uint32_t *dest, uint32_t *source, size_t size)
 {
@@ -148,7 +121,7 @@ static uint32_t *mxc_nand_memcpy32(uint32_t *dest, uint32_t *source, size_t size
 
 /*
  * This function polls the NANDFC to wait for the basic operation to
- * complete by checking the INT bit of config2 register.
+ * complete by checking the INT bit.
  */
 static void wait_op_done(struct mxc_nand_host *host, int max_retries,
 				uint16_t param)
@@ -156,10 +129,17 @@ static void wait_op_done(struct mxc_nand_host *host, int max_retries,
 	uint32_t tmp;
 
 	while (max_retries-- > 0) {
-		if (readw(&host->regs->config2) & NFC_INT) {
-			tmp = readw(&host->regs->config2);
-			tmp  &= ~NFC_INT;
-			writew(tmp, &host->regs->config2);
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+		tmp = readnfc(&host->regs->config2);
+		if (tmp & NFC_V1_V2_CONFIG2_INT) {
+			tmp &= ~NFC_V1_V2_CONFIG2_INT;
+			writenfc(tmp, &host->regs->config2);
+#elif defined(MXC_NFC_V3_2)
+		tmp = readnfc(&host->ip_regs->ipc);
+		if (tmp & NFC_V3_IPC_INT) {
+			tmp &= ~NFC_V3_IPC_INT;
+			writenfc(tmp, &host->ip_regs->ipc);
+#endif
 			break;
 		}
 		udelay(1);
@@ -178,8 +158,8 @@ static void send_cmd(struct mxc_nand_host *host, uint16_t cmd)
 {
 	MTDDEBUG(MTD_DEBUG_LEVEL3, "send_cmd(host, 0x%x)\n", cmd);
 
-	writew(cmd, &host->regs->flash_cmd);
-	writew(NFC_CMD, &host->regs->config2);
+	writenfc(cmd, &host->regs->flash_cmd);
+	writenfc(NFC_CMD, &host->regs->operation);
 
 	/* Wait for operation to complete */
 	wait_op_done(host, TROP_US_DELAY, cmd);
@@ -194,8 +174,8 @@ static void send_addr(struct mxc_nand_host *host, uint16_t addr)
 {
 	MTDDEBUG(MTD_DEBUG_LEVEL3, "send_addr(host, 0x%x)\n", addr);
 
-	writew(addr, &host->regs->flash_addr);
-	writew(NFC_ADDR, &host->regs->config2);
+	writenfc(addr, &host->regs->flash_addr);
+	writenfc(NFC_ADDR, &host->regs->operation);
 
 	/* Wait for operation to complete */
 	wait_op_done(host, TROP_US_DELAY, addr);
@@ -211,7 +191,7 @@ static void send_prog_page(struct mxc_nand_host *host, uint8_t buf_id,
 	if (spare_only)
 		MTDDEBUG(MTD_DEBUG_LEVEL1, "send_prog_page (%d)\n", spare_only);
 
-	if (is_mxc_nfc_21()) {
+	if (is_mxc_nfc_21() || is_mxc_nfc_32()) {
 		int i;
 		/*
 		 *  The controller copies the 64 bytes of spare data from
@@ -227,19 +207,26 @@ static void send_prog_page(struct mxc_nand_host *host, uint8_t buf_id,
 		}
 	}
 
-	writew(buf_id, &host->regs->buf_addr);
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+	writenfc(buf_id, &host->regs->buf_addr);
+#elif defined(MXC_NFC_V3_2)
+	uint32_t tmp = readnfc(&host->regs->config1);
+	tmp &= ~NFC_V3_CONFIG1_RBA_MASK;
+	tmp |= NFC_V3_CONFIG1_RBA(buf_id);
+	writenfc(tmp, &host->regs->config1);
+#endif
 
 	/* Configure spare or page+spare access */
 	if (!host->pagesize_2k) {
-		uint16_t config1 = readw(&host->regs->config1);
+		uint32_t config1 = readnfc(&host->regs->config1);
 		if (spare_only)
-			config1 |= NFC_SP_EN;
+			config1 |= NFC_CONFIG1_SP_EN;
 		else
-			config1 &= ~NFC_SP_EN;
-		writew(config1, &host->regs->config1);
+			config1 &= ~NFC_CONFIG1_SP_EN;
+		writenfc(config1, &host->regs->config1);
 	}
 
-	writew(NFC_INPUT, &host->regs->config2);
+	writenfc(NFC_INPUT, &host->regs->operation);
 
 	/* Wait for operation to complete */
 	wait_op_done(host, TROP_US_DELAY, spare_only);
@@ -254,24 +241,31 @@ static void send_read_page(struct mxc_nand_host *host, uint8_t buf_id,
 {
 	MTDDEBUG(MTD_DEBUG_LEVEL3, "send_read_page (%d)\n", spare_only);
 
-	writew(buf_id, &host->regs->buf_addr);
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+	writenfc(buf_id, &host->regs->buf_addr);
+#elif defined(MXC_NFC_V3_2)
+	uint32_t tmp = readnfc(&host->regs->config1);
+	tmp &= ~NFC_V3_CONFIG1_RBA_MASK;
+	tmp |= NFC_V3_CONFIG1_RBA(buf_id);
+	writenfc(tmp, &host->regs->config1);
+#endif
 
 	/* Configure spare or page+spare access */
 	if (!host->pagesize_2k) {
-		uint32_t config1 = readw(&host->regs->config1);
+		uint32_t config1 = readnfc(&host->regs->config1);
 		if (spare_only)
-			config1 |= NFC_SP_EN;
+			config1 |= NFC_CONFIG1_SP_EN;
 		else
-			config1 &= ~NFC_SP_EN;
-		writew(config1, &host->regs->config1);
+			config1 &= ~NFC_CONFIG1_SP_EN;
+		writenfc(config1, &host->regs->config1);
 	}
 
-	writew(NFC_OUTPUT, &host->regs->config2);
+	writenfc(NFC_OUTPUT, &host->regs->operation);
 
 	/* Wait for operation to complete */
 	wait_op_done(host, TROP_US_DELAY, spare_only);
 
-	if (is_mxc_nfc_21()) {
+	if (is_mxc_nfc_21() || is_mxc_nfc_32()) {
 		int i;
 
 		/*
@@ -291,17 +285,23 @@ static void send_read_page(struct mxc_nand_host *host, uint8_t buf_id,
 /* Request the NANDFC to perform a read of the NAND device ID. */
 static void send_read_id(struct mxc_nand_host *host)
 {
-	uint16_t tmp;
+	uint32_t tmp;
 
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
 	/* NANDFC buffer 0 is used for device ID output */
-	writew(0x0, &host->regs->buf_addr);
+	writenfc(0x0, &host->regs->buf_addr);
+#elif defined(MXC_NFC_V3_2)
+	tmp = readnfc(&host->regs->config1);
+	tmp &= ~NFC_V3_CONFIG1_RBA_MASK;
+	writenfc(tmp, &host->regs->config1);
+#endif
 
 	/* Read ID into main buffer */
-	tmp = readw(&host->regs->config1);
-	tmp &= ~NFC_SP_EN;
-	writew(tmp, &host->regs->config1);
+	tmp = readnfc(&host->regs->config1);
+	tmp &= ~NFC_CONFIG1_SP_EN;
+	writenfc(tmp, &host->regs->config1);
 
-	writew(NFC_ID, &host->regs->config2);
+	writenfc(NFC_ID, &host->regs->operation);
 
 	/* Wait for operation to complete */
 	wait_op_done(host, TROP_US_DELAY, 0);
@@ -313,32 +313,40 @@ static void send_read_id(struct mxc_nand_host *host)
  */
 static uint16_t get_dev_status(struct mxc_nand_host *host)
 {
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
 	void __iomem *main_buf = host->regs->main_area[1];
 	uint32_t store;
-	uint16_t ret, tmp;
+#endif
+	uint32_t ret, tmp;
 	/* Issue status request to NAND device */
 
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
 	/* store the main area1 first word, later do recovery */
 	store = readl(main_buf);
 	/* NANDFC buffer 1 is used for device status */
-	writew(1, &host->regs->buf_addr);
+	writenfc(1, &host->regs->buf_addr);
+#endif
 
 	/* Read status into main buffer */
-	tmp = readw(&host->regs->config1);
-	tmp &= ~NFC_SP_EN;
-	writew(tmp, &host->regs->config1);
+	tmp = readnfc(&host->regs->config1);
+	tmp &= ~NFC_CONFIG1_SP_EN;
+	writenfc(tmp, &host->regs->config1);
 
-	writew(NFC_STATUS, &host->regs->config2);
+	writenfc(NFC_STATUS, &host->regs->operation);
 
 	/* Wait for operation to complete */
 	wait_op_done(host, TROP_US_DELAY, 0);
 
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
 	/*
 	 *  Status is placed in first word of main buffer
 	 * get status, then recovery area 1 data
 	 */
 	ret = readw(main_buf);
 	writel(store, main_buf);
+#elif defined(MXC_NFC_V3_2)
+	ret = readnfc(&host->regs->config1) >> 16;
+#endif
 
 	return ret;
 }
@@ -357,13 +365,23 @@ static void _mxc_nand_enable_hwecc(struct mtd_info *mtd, int on)
 {
 	struct nand_chip *nand_chip = mtd->priv;
 	struct mxc_nand_host *host = nand_chip->priv;
-	uint16_t tmp = readw(&host->regs->config1);
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+	uint16_t tmp = readnfc(&host->regs->config1);
 
 	if (on)
-		tmp |= NFC_ECC_EN;
+		tmp |= NFC_V1_V2_CONFIG1_ECC_EN;
 	else
-		tmp &= ~NFC_ECC_EN;
-	writew(tmp, &host->regs->config1);
+		tmp &= ~NFC_V1_V2_CONFIG1_ECC_EN;
+	writenfc(tmp, &host->regs->config1);
+#elif defined(MXC_NFC_V3_2)
+	uint32_t tmp = readnfc(&host->ip_regs->config2);
+
+	if (on)
+		tmp |= NFC_V3_CONFIG2_ECC_EN;
+	else
+		tmp &= ~NFC_V3_CONFIG2_ECC_EN;
+	writenfc(tmp, &host->ip_regs->config2);
+#endif
 }
 
 #ifdef CONFIG_MXC_NAND_HWECC
@@ -375,7 +393,7 @@ static void mxc_nand_enable_hwecc(struct mtd_info *mtd, int mode)
 	 */
 }
 
-#ifdef MXC_NFC_V2_1
+#if defined(MXC_NFC_V2_1) || defined(MXC_NFC_V3_2)
 static int mxc_nand_read_oob_syndrome(struct mtd_info *mtd,
 				      struct nand_chip *chip,
 				      int page, int sndcmd)
@@ -389,7 +407,7 @@ static int mxc_nand_read_oob_syndrome(struct mtd_info *mtd,
 
 	MTDDEBUG(MTD_DEBUG_LEVEL0,
 			"%s: Reading OOB area of page %u to oob %p\n",
-			 __FUNCTION__, host->page_addr, buf);
+			 __func__, page, buf);
 
 	chip->cmdfunc(mtd, NAND_CMD_READOOB, mtd->writesize, page);
 	for (i = 0; i < chip->ecc.steps; i++) {
@@ -443,7 +461,7 @@ static int mxc_nand_read_page_raw_syndrome(struct mtd_info *mtd,
 	int n;
 
 	_mxc_nand_enable_hwecc(mtd, 0);
-	chip->cmdfunc(mtd, NAND_CMD_READ0, 0x00, host->page_addr);
+	chip->cmdfunc(mtd, NAND_CMD_READ0, 0x00, page);
 
 	for (n = 0, steps = chip->ecc.steps; steps > 0; n++, steps--) {
 		host->col_addr = n * eccsize;
@@ -487,7 +505,7 @@ static int mxc_nand_read_page_syndrome(struct mtd_info *mtd,
 	uint8_t *oob = chip->oob_poi;
 
 	MTDDEBUG(MTD_DEBUG_LEVEL1, "Reading page %u to buf %p oob %p\n",
-	      host->page_addr, buf, oob);
+	      page, buf, oob);
 
 	/* first read the data area and the available portion of OOB */
 	for (n = 0; eccsteps; n++, eccsteps--, p += eccsize) {
@@ -525,7 +543,7 @@ static int mxc_nand_read_page_syndrome(struct mtd_info *mtd,
 
 	/* Then switch ECC off and read the OOB area to get the ECC code */
 	_mxc_nand_enable_hwecc(mtd, 0);
-	chip->cmdfunc(mtd, NAND_CMD_READOOB, mtd->writesize, host->page_addr);
+	chip->cmdfunc(mtd, NAND_CMD_READOOB, mtd->writesize, page);
 	eccsteps = chip->ecc.steps;
 	oob = chip->oob_poi + chip->ecc.prepad;
 	for (n = 0; eccsteps; n++, eccsteps--, p += eccsize) {
@@ -696,7 +714,7 @@ static int mxc_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 	 * additional correction.  2-Bit errors cannot be corrected by
 	 * HW ECC, so we need to return failure
 	 */
-	uint16_t ecc_status = readw(&host->regs->ecc_status_result);
+	uint16_t ecc_status = readnfc(&host->regs->ecc_status_result);
 
 	if (((ecc_status & 0x3) == 2) || ((ecc_status >> 2) == 2)) {
 		MTDDEBUG(MTD_DEBUG_LEVEL0,
@@ -1165,8 +1183,8 @@ static struct nand_bbt_descr bbt_mirror_descr = {
 int board_nand_init(struct nand_chip *this)
 {
 	struct mtd_info *mtd;
-#ifdef MXC_NFC_V2_1
-	uint16_t tmp;
+#if defined(MXC_NFC_V2_1) || defined(MXC_NFC_V3_2)
+	uint32_t tmp;
 #endif
 
 #ifdef CONFIG_SYS_NAND_USE_FLASH_BBT
@@ -1193,14 +1211,18 @@ int board_nand_init(struct nand_chip *this)
 	this->read_buf = mxc_nand_read_buf;
 	this->verify_buf = mxc_nand_verify_buf;
 
-	host->regs = (struct fsl_nfc_regs __iomem *)CONFIG_MXC_NAND_REGS_BASE;
+	host->regs = (struct mxc_nand_regs __iomem *)CONFIG_MXC_NAND_REGS_BASE;
+#ifdef MXC_NFC_V3_2
+	host->ip_regs =
+		(struct mxc_nand_ip_regs __iomem *)CONFIG_MXC_NAND_IP_REGS_BASE;
+#endif
 	host->clk_act = 1;
 
 #ifdef CONFIG_MXC_NAND_HWECC
 	this->ecc.calculate = mxc_nand_calculate_ecc;
 	this->ecc.hwctl = mxc_nand_enable_hwecc;
 	this->ecc.correct = mxc_nand_correct_data;
-	if (is_mxc_nfc_21()) {
+	if (is_mxc_nfc_21() || is_mxc_nfc_32()) {
 		this->ecc.mode = NAND_ECC_HW_SYNDROME;
 		this->ecc.read_page = mxc_nand_read_page_syndrome;
 		this->ecc.read_page_raw = mxc_nand_read_page_raw_syndrome;
@@ -1238,25 +1260,26 @@ int board_nand_init(struct nand_chip *this)
 	this->ecc.layout = &nand_hw_eccoob;
 #endif
 
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
 #ifdef MXC_NFC_V2_1
-	tmp = readw(&host->regs->config1);
-	tmp |= NFC_ONE_CYCLE;
-	tmp |= NFC_4_8N_ECC;
-	writew(tmp, &host->regs->config1);
+	tmp = readnfc(&host->regs->config1);
+	tmp |= NFC_V2_CONFIG1_ONE_CYCLE;
+	tmp |= NFC_V2_CONFIG1_ECC_MODE_4;
+	writenfc(tmp, &host->regs->config1);
 	if (host->pagesize_2k)
-		writew(64/2, &host->regs->spare_area_size);
+		writenfc(64/2, &host->regs->spare_area_size);
 	else
-		writew(16/2, &host->regs->spare_area_size);
+		writenfc(16/2, &host->regs->spare_area_size);
 #endif
 
 	/*
 	 * preset operation
 	 * Unlock the internal RAM Buffer
 	 */
-	writew(0x2, &host->regs->config);
+	writenfc(0x2, &host->regs->config);
 
 	/* Blocks to be unlocked */
-	writew(0x0, &host->regs->unlockstart_blkaddr);
+	writenfc(0x0, &host->regs->unlockstart_blkaddr);
 	/* Originally (Freescale LTIB 2.6.21) 0x4000 was written to the
 	 * unlockend_blkaddr, but the magic 0x4000 does not always work
 	 * when writing more than some 32 megabytes (on 2k page nands)
@@ -1268,10 +1291,53 @@ int board_nand_init(struct nand_chip *this)
 	 * This might be NAND chip specific and the i.MX31 datasheet is
 	 * extremely vague about the semantics of this register.
 	 */
-	writew(0xFFFF, &host->regs->unlockend_blkaddr);
+	writenfc(0xFFFF, &host->regs->unlockend_blkaddr);
 
 	/* Unlock Block Command for given address range */
-	writew(0x4, &host->regs->wrprot);
+	writenfc(0x4, &host->regs->wrprot);
+#elif defined(MXC_NFC_V3_2)
+	writenfc(NFC_V3_CONFIG1_RBA(0), &host->regs->config1);
+	writenfc(NFC_V3_IPC_CREQ, &host->ip_regs->ipc);
+
+	/* Unlock the internal RAM Buffer */
+	writenfc(NFC_V3_WRPROT_BLS_UNLOCK | NFC_V3_WRPROT_UNLOCK,
+			&host->ip_regs->wrprot);
+
+	/* Blocks to be unlocked */
+	for (tmp = 0; tmp < CONFIG_SYS_NAND_MAX_CHIPS; tmp++)
+		writenfc(0x0 | 0xFFFF << 16,
+				&host->ip_regs->wrprot_unlock_blkaddr[tmp]);
+
+	writenfc(0, &host->ip_regs->ipc);
+
+	tmp = readnfc(&host->ip_regs->config2);
+	tmp &= ~(NFC_V3_CONFIG2_SPAS_MASK | NFC_V3_CONFIG2_EDC_MASK |
+			NFC_V3_CONFIG2_ECC_MODE_8 | NFC_V3_CONFIG2_PS_MASK);
+	tmp |= NFC_V3_CONFIG2_ONE_CYCLE;
+
+	if (host->pagesize_2k) {
+		tmp |= NFC_V3_CONFIG2_SPAS(64/2);
+		tmp |= NFC_V3_CONFIG2_PS_2048;
+	} else {
+		tmp |= NFC_V3_CONFIG2_SPAS(16/2);
+		tmp |= NFC_V3_CONFIG2_PS_512;
+	}
+
+	writenfc(tmp, &host->ip_regs->config2);
+
+	tmp = NFC_V3_CONFIG3_NUM_OF_DEVS(0) |
+			NFC_V3_CONFIG3_NO_SDMA |
+			NFC_V3_CONFIG3_RBB_MODE |
+			NFC_V3_CONFIG3_SBB(6) | /* Reset default */
+			NFC_V3_CONFIG3_ADD_OP(0);
+
+	if (!(this->options & NAND_BUSWIDTH_16))
+		tmp |= NFC_V3_CONFIG3_FW8;
+
+	writenfc(tmp, &host->ip_regs->config3);
+
+	writenfc(0, &host->ip_regs->delay_line);
+#endif
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/mxc_nand.h b/drivers/mtd/nand/mxc_nand.h
new file mode 100644
index 0000000000..308ff8d8a8
--- /dev/null
+++ b/drivers/mtd/nand/mxc_nand.h
@@ -0,0 +1,225 @@
+/*
+ * (c) 2009 Magnus Lilja <lilja.magnus@gmail.com>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * 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_NAND_H
+#define __MXC_NAND_H
+
+/*
+ * Register map and bit definitions for the Freescale NAND Flash Controller
+ * present in various i.MX devices.
+ *
+ * MX31 and MX27 have version 1, which has:
+ *	4 512-byte main buffers and
+ *	4 16-byte spare buffers
+ *	to support up to 2K byte pagesize nand.
+ *	Reading or writing a 2K page requires 4 FDI/FDO cycles.
+ *
+ * MX25 and MX35 have version 2.1, and MX51 and MX53 have version 3.2, which
+ * have:
+ *	8 512-byte main buffers and
+ *	8 64-byte spare buffers
+ *	to support up to 4K byte pagesize nand.
+ *	Reading or writing a 2K or 4K page requires only 1 FDI/FDO cycle.
+ *	Also some of registers are moved and/or changed meaning as seen below.
+ */
+#if defined(CONFIG_MX27) || defined(CONFIG_MX31)
+#define MXC_NFC_V1
+#define is_mxc_nfc_1()		1
+#define is_mxc_nfc_21()		0
+#define is_mxc_nfc_32()		0
+#elif defined(CONFIG_MX25) || defined(CONFIG_MX35)
+#define MXC_NFC_V2_1
+#define is_mxc_nfc_1()		0
+#define is_mxc_nfc_21()		1
+#define is_mxc_nfc_32()		0
+#elif defined(CONFIG_MX51) || defined(CONFIG_MX53)
+#define MXC_NFC_V3
+#define MXC_NFC_V3_2
+#define is_mxc_nfc_1()		0
+#define is_mxc_nfc_21()		0
+#define is_mxc_nfc_32()		1
+#else
+#error "MXC NFC implementation not supported"
+#endif
+#define is_mxc_nfc_3()		is_mxc_nfc_32()
+
+#if defined(MXC_NFC_V1)
+#define NAND_MXC_NR_BUFS		4
+#define NAND_MXC_SPARE_BUF_SIZE		16
+#define NAND_MXC_REG_OFFSET		0xe00
+#define NAND_MXC_2K_MULTI_CYCLE
+#elif defined(MXC_NFC_V2_1) || defined(MXC_NFC_V3_2)
+#define NAND_MXC_NR_BUFS		8
+#define NAND_MXC_SPARE_BUF_SIZE		64
+#define NAND_MXC_REG_OFFSET		0x1e00
+#endif
+
+struct mxc_nand_regs {
+	u8 main_area[NAND_MXC_NR_BUFS][0x200];
+	u8 spare_area[NAND_MXC_NR_BUFS][NAND_MXC_SPARE_BUF_SIZE];
+	/*
+	 * reserved size is offset of nfc registers
+	 * minus total main and spare sizes
+	 */
+	u8 reserved1[NAND_MXC_REG_OFFSET
+		- NAND_MXC_NR_BUFS * (512 + NAND_MXC_SPARE_BUF_SIZE)];
+#if defined(MXC_NFC_V1)
+	u16 buf_size;
+	u16 reserved2;
+	u16 buf_addr;
+	u16 flash_addr;
+	u16 flash_cmd;
+	u16 config;
+	u16 ecc_status_result;
+	u16 rsltmain_area;
+	u16 rsltspare_area;
+	u16 wrprot;
+	u16 unlockstart_blkaddr;
+	u16 unlockend_blkaddr;
+	u16 nf_wrprst;
+	u16 config1;
+	u16 config2;
+#elif defined(MXC_NFC_V2_1)
+	u16 reserved2[2];
+	u16 buf_addr;
+	u16 flash_addr;
+	u16 flash_cmd;
+	u16 config;
+	u32 ecc_status_result;
+	u16 spare_area_size;
+	u16 wrprot;
+	u16 reserved3[2];
+	u16 nf_wrprst;
+	u16 config1;
+	u16 config2;
+	u16 reserved4;
+	u16 unlockstart_blkaddr;
+	u16 unlockend_blkaddr;
+	u16 unlockstart_blkaddr1;
+	u16 unlockend_blkaddr1;
+	u16 unlockstart_blkaddr2;
+	u16 unlockend_blkaddr2;
+	u16 unlockstart_blkaddr3;
+	u16 unlockend_blkaddr3;
+#elif defined(MXC_NFC_V3_2)
+	u32 flash_cmd;
+	u32 flash_addr[12];
+	u32 config1;
+	u32 ecc_status_result;
+	u32 status_sum;
+	u32 launch;
+#endif
+};
+
+#ifdef MXC_NFC_V3_2
+struct mxc_nand_ip_regs {
+	u32 wrprot;
+	u32 wrprot_unlock_blkaddr[8];
+	u32 config2;
+	u32 config3;
+	u32 ipc;
+	u32 err_addr;
+	u32 delay_line;
+};
+#endif
+
+/* Set FCMD to 1, rest to 0 for Command operation */
+#define NFC_CMD				0x1
+
+/* Set FADD to 1, rest to 0 for Address operation */
+#define NFC_ADDR			0x2
+
+/* Set FDI to 1, rest to 0 for Input operation */
+#define NFC_INPUT			0x4
+
+/* Set FDO to 001, rest to 0 for Data Output operation */
+#define NFC_OUTPUT			0x8
+
+/* Set FDO to 010, rest to 0 for Read ID operation */
+#define NFC_ID				0x10
+
+/* Set FDO to 100, rest to 0 for Read Status operation */
+#define NFC_STATUS			0x20
+
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+#define NFC_CONFIG1_SP_EN		(1 << 2)
+#define NFC_CONFIG1_RST			(1 << 6)
+#define NFC_CONFIG1_CE			(1 << 7)
+#elif defined(MXC_NFC_V3_2)
+#define NFC_CONFIG1_SP_EN		(1 << 0)
+#define NFC_CONFIG1_CE			(1 << 1)
+#define NFC_CONFIG1_RST			(1 << 2)
+#endif
+#define NFC_V1_V2_CONFIG1_ECC_EN	(1 << 3)
+#define NFC_V1_V2_CONFIG1_INT_MSK	(1 << 4)
+#define NFC_V1_V2_CONFIG1_BIG		(1 << 5)
+#define NFC_V2_CONFIG1_ECC_MODE_4	(1 << 0)
+#define NFC_V2_CONFIG1_ONE_CYCLE	(1 << 8)
+#define NFC_V2_CONFIG1_FP_INT		(1 << 11)
+#define NFC_V3_CONFIG1_RBA_MASK		(0x7 << 4)
+#define NFC_V3_CONFIG1_RBA(x)		(((x) & 0x7) << 4)
+
+#define NFC_V1_V2_CONFIG2_INT		(1 << 15)
+#define NFC_V3_CONFIG2_PS_MASK		(0x3 << 0)
+#define NFC_V3_CONFIG2_PS_512		(0 << 0)
+#define NFC_V3_CONFIG2_PS_2048		(1 << 0)
+#define NFC_V3_CONFIG2_PS_4096		(2 << 0)
+#define NFC_V3_CONFIG2_ONE_CYCLE	(1 << 2)
+#define NFC_V3_CONFIG2_ECC_EN		(1 << 3)
+#define NFC_V3_CONFIG2_2CMD_PHASES	(1 << 4)
+#define NFC_V3_CONFIG2_NUM_ADDR_PH0	(1 << 5)
+#define NFC_V3_CONFIG2_ECC_MODE_8	(1 << 6)
+#define NFC_V3_CONFIG2_PPB_MASK		(0x3 << 7)
+#define NFC_V3_CONFIG2_PPB(x)		(((x) & 0x3) << 7)
+#define NFC_V3_CONFIG2_EDC_MASK		(0x7 << 9)
+#define NFC_V3_CONFIG2_EDC(x)		(((x) & 0x7) << 9)
+#define NFC_V3_CONFIG2_NUM_ADDR_PH1(x)	(((x) & 0x3) << 12)
+#define NFC_V3_CONFIG2_INT_MSK		(1 << 15)
+#define NFC_V3_CONFIG2_SPAS_MASK	(0xff << 16)
+#define NFC_V3_CONFIG2_SPAS(x)		(((x) & 0xff) << 16)
+#define NFC_V3_CONFIG2_ST_CMD_MASK	(0xff << 24)
+#define NFC_V3_CONFIG2_ST_CMD(x)	(((x) & 0xff) << 24)
+
+#define NFC_V3_CONFIG3_ADD_OP(x)	(((x) & 0x3) << 0)
+#define NFC_V3_CONFIG3_FW8		(1 << 3)
+#define NFC_V3_CONFIG3_SBB(x)		(((x) & 0x7) << 8)
+#define NFC_V3_CONFIG3_NUM_OF_DEVS(x)	(((x) & 0x7) << 12)
+#define NFC_V3_CONFIG3_RBB_MODE		(1 << 15)
+#define NFC_V3_CONFIG3_NO_SDMA		(1 << 20)
+
+#define NFC_V3_WRPROT_UNLOCK		(1 << 2)
+#define NFC_V3_WRPROT_BLS_UNLOCK	(2 << 6)
+
+#define NFC_V3_IPC_CREQ			(1 << 0)
+#define NFC_V3_IPC_INT			(1 << 31)
+
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+#define operation	config2
+#define readnfc		readw
+#define writenfc	writew
+#elif defined(MXC_NFC_V3_2)
+#define operation	launch
+#define readnfc		readl
+#define writenfc	writel
+#endif
+
+#endif /* __MXC_NAND_H */
diff --git a/drivers/mtd/nand/mxc_nand_spl.c b/drivers/mtd/nand/mxc_nand_spl.c
new file mode 100644
index 0000000000..09f23c30c4
--- /dev/null
+++ b/drivers/mtd/nand/mxc_nand_spl.c
@@ -0,0 +1,366 @@
+/*
+ * (C) Copyright 2009
+ * Magnus Lilja <lilja.magnus@gmail.com>
+ *
+ * (C) Copyright 2008
+ * Maxim Artamonov, <scn1874 at yandex.ru>
+ *
+ * (C) Copyright 2006-2008
+ * Stefan Roese, DENX Software Engineering, sr at denx.de.
+ *
+ * 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 <common.h>
+#include <nand.h>
+#include <asm/arch/imx-regs.h>
+#include <asm/io.h>
+#include "mxc_nand.h"
+
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+static struct mxc_nand_regs *const nfc = (void *)NFC_BASE_ADDR;
+#elif defined(MXC_NFC_V3_2)
+static struct mxc_nand_regs *const nfc = (void *)NFC_BASE_ADDR_AXI;
+static struct mxc_nand_ip_regs *const nfc_ip = (void *)NFC_BASE_ADDR;
+#endif
+
+static void nfc_wait_ready(void)
+{
+	uint32_t tmp;
+
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+	while (!(readnfc(&nfc->config2) & NFC_V1_V2_CONFIG2_INT))
+		;
+
+	/* Reset interrupt flag */
+	tmp = readnfc(&nfc->config2);
+	tmp &= ~NFC_V1_V2_CONFIG2_INT;
+	writenfc(tmp, &nfc->config2);
+#elif defined(MXC_NFC_V3_2)
+	while (!(readnfc(&nfc_ip->ipc) & NFC_V3_IPC_INT))
+		;
+
+	/* Reset interrupt flag */
+	tmp = readnfc(&nfc_ip->ipc);
+	tmp &= ~NFC_V3_IPC_INT;
+	writenfc(tmp, &nfc_ip->ipc);
+#endif
+}
+
+static void nfc_nand_init(void)
+{
+#if defined(MXC_NFC_V3_2)
+	int ecc_per_page = CONFIG_SYS_NAND_PAGE_SIZE / 512;
+	int tmp;
+
+	tmp = (readnfc(&nfc_ip->config2) & ~(NFC_V3_CONFIG2_SPAS_MASK |
+			NFC_V3_CONFIG2_EDC_MASK | NFC_V3_CONFIG2_PS_MASK)) |
+		NFC_V3_CONFIG2_SPAS(CONFIG_SYS_NAND_OOBSIZE / 2) |
+		NFC_V3_CONFIG2_INT_MSK | NFC_V3_CONFIG2_ECC_EN |
+		NFC_V3_CONFIG2_ONE_CYCLE;
+	if (CONFIG_SYS_NAND_PAGE_SIZE == 4096)
+		tmp |= NFC_V3_CONFIG2_PS_4096;
+	else if (CONFIG_SYS_NAND_PAGE_SIZE == 2048)
+		tmp |= NFC_V3_CONFIG2_PS_2048;
+	else if (CONFIG_SYS_NAND_PAGE_SIZE == 512)
+		tmp |= NFC_V3_CONFIG2_PS_512;
+	/*
+	 * if spare size is larger that 16 bytes per 512 byte hunk
+	 * then use 8 symbol correction instead of 4
+	 */
+	if (CONFIG_SYS_NAND_OOBSIZE / ecc_per_page > 16)
+		tmp |= NFC_V3_CONFIG2_ECC_MODE_8;
+	else
+		tmp &= ~NFC_V3_CONFIG2_ECC_MODE_8;
+	writenfc(tmp, &nfc_ip->config2);
+
+	tmp = NFC_V3_CONFIG3_NUM_OF_DEVS(0) |
+			NFC_V3_CONFIG3_NO_SDMA |
+			NFC_V3_CONFIG3_RBB_MODE |
+			NFC_V3_CONFIG3_SBB(6) | /* Reset default */
+			NFC_V3_CONFIG3_ADD_OP(0);
+#ifndef CONFIG_SYS_NAND_BUSWIDTH_16
+	tmp |= NFC_V3_CONFIG3_FW8;
+#endif
+	writenfc(tmp, &nfc_ip->config3);
+
+	writenfc(0, &nfc_ip->delay_line);
+#elif defined(MXC_NFC_V2_1)
+	int ecc_per_page = CONFIG_SYS_NAND_PAGE_SIZE / 512;
+	int config1;
+
+	writenfc(CONFIG_SYS_NAND_OOBSIZE / 2, &nfc->spare_area_size);
+
+	/* unlocking RAM Buff */
+	writenfc(0x2, &nfc->config);
+
+	/* hardware ECC checking and correct */
+	config1 = readnfc(&nfc->config1) | NFC_V1_V2_CONFIG1_ECC_EN |
+			NFC_V1_V2_CONFIG1_INT_MSK | NFC_V2_CONFIG1_ONE_CYCLE |
+			NFC_V2_CONFIG1_FP_INT;
+	/*
+	 * if spare size is larger that 16 bytes per 512 byte hunk
+	 * then use 8 symbol correction instead of 4
+	 */
+	if (CONFIG_SYS_NAND_OOBSIZE / ecc_per_page > 16)
+		config1 &= ~NFC_V2_CONFIG1_ECC_MODE_4;
+	else
+		config1 |= NFC_V2_CONFIG1_ECC_MODE_4;
+	writenfc(config1, &nfc->config1);
+#elif defined(MXC_NFC_V1)
+	/* unlocking RAM Buff */
+	writenfc(0x2, &nfc->config);
+
+	/* hardware ECC checking and correct */
+	writenfc(NFC_V1_V2_CONFIG1_ECC_EN | NFC_V1_V2_CONFIG1_INT_MSK,
+			&nfc->config1);
+#endif
+}
+
+static void nfc_nand_command(unsigned short command)
+{
+	writenfc(command, &nfc->flash_cmd);
+	writenfc(NFC_CMD, &nfc->operation);
+	nfc_wait_ready();
+}
+
+static void nfc_nand_address(unsigned short address)
+{
+	writenfc(address, &nfc->flash_addr);
+	writenfc(NFC_ADDR, &nfc->operation);
+	nfc_wait_ready();
+}
+
+static void nfc_nand_page_address(unsigned int page_address)
+{
+	unsigned int page_count;
+
+	nfc_nand_address(0x00);
+
+	/* code only for large page flash */
+	if (CONFIG_SYS_NAND_PAGE_SIZE > 512)
+		nfc_nand_address(0x00);
+
+	page_count = CONFIG_SYS_NAND_SIZE / CONFIG_SYS_NAND_PAGE_SIZE;
+
+	if (page_address <= page_count) {
+		page_count--; /* transform 0x01000000 to 0x00ffffff */
+		do {
+			nfc_nand_address(page_address & 0xff);
+			page_address = page_address >> 8;
+			page_count = page_count >> 8;
+		} while (page_count);
+	}
+
+	nfc_nand_address(0x00);
+}
+
+static void nfc_nand_data_output(void)
+{
+#ifdef NAND_MXC_2K_MULTI_CYCLE
+	int i;
+#endif
+
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+	writenfc(0, &nfc->buf_addr);
+#elif defined(MXC_NFC_V3_2)
+	int config1 = readnfc(&nfc->config1);
+	config1 &= ~NFC_V3_CONFIG1_RBA_MASK;
+	writenfc(config1, &nfc->config1);
+#endif
+	writenfc(NFC_OUTPUT, &nfc->operation);
+	nfc_wait_ready();
+#ifdef NAND_MXC_2K_MULTI_CYCLE
+	/*
+	 * This NAND controller requires multiple input commands
+	 * for pages larger than 512 bytes.
+	 */
+	for (i = 1; i < CONFIG_SYS_NAND_PAGE_SIZE / 512; i++) {
+		writenfc(i, &nfc->buf_addr);
+		writenfc(NFC_OUTPUT, &nfc->operation);
+		nfc_wait_ready();
+	}
+#endif
+}
+
+static int nfc_nand_check_ecc(void)
+{
+#if defined(MXC_NFC_V1)
+	u16 ecc_status = readw(&nfc->ecc_status_result);
+	return (ecc_status & 0x3) == 2 || (ecc_status >> 2) == 2;
+#elif defined(MXC_NFC_V2_1) || defined(MXC_NFC_V3_2)
+	u32 ecc_status = readl(&nfc->ecc_status_result);
+	int ecc_per_page = CONFIG_SYS_NAND_PAGE_SIZE / 512;
+	int err_limit = CONFIG_SYS_NAND_OOBSIZE / ecc_per_page > 16 ? 8 : 4;
+	int subpages = CONFIG_SYS_NAND_PAGE_SIZE / 512;
+
+	do {
+		if ((ecc_status & 0xf) > err_limit)
+			return 1;
+		ecc_status >>= 4;
+	} while (--subpages);
+
+	return 0;
+#endif
+}
+
+static void nfc_nand_read_page(unsigned int page_address)
+{
+	/* read in first 0 buffer */
+#if defined(MXC_NFC_V1) || defined(MXC_NFC_V2_1)
+	writenfc(0, &nfc->buf_addr);
+#elif defined(MXC_NFC_V3_2)
+	int config1 = readnfc(&nfc->config1);
+	config1 &= ~NFC_V3_CONFIG1_RBA_MASK;
+	writenfc(config1, &nfc->config1);
+#endif
+	nfc_nand_command(NAND_CMD_READ0);
+	nfc_nand_page_address(page_address);
+
+	if (CONFIG_SYS_NAND_PAGE_SIZE > 512)
+		nfc_nand_command(NAND_CMD_READSTART);
+
+	nfc_nand_data_output(); /* fill the main buffer 0 */
+}
+
+static int nfc_read_page(unsigned int page_address, unsigned char *buf)
+{
+	int i;
+	u32 *src;
+	u32 *dst;
+
+	nfc_nand_read_page(page_address);
+
+	if (nfc_nand_check_ecc())
+		return -1;
+
+	src = (u32 *)&nfc->main_area[0][0];
+	dst = (u32 *)buf;
+
+	/* main copy loop from NAND-buffer to SDRAM memory */
+	for (i = 0; i < CONFIG_SYS_NAND_PAGE_SIZE / 4; i++) {
+		writel(readl(src), dst);
+		src++;
+		dst++;
+	}
+
+	return 0;
+}
+
+static int is_badblock(int pagenumber)
+{
+	int page = pagenumber;
+	u32 badblock;
+	u32 *src;
+
+	/* Check the first two pages for bad block markers */
+	for (page = pagenumber; page < pagenumber + 2; page++) {
+		nfc_nand_read_page(page);
+
+		src = (u32 *)&nfc->spare_area[0][0];
+
+		/*
+		 * IMPORTANT NOTE: The nand flash controller uses a non-
+		 * standard layout for large page devices. This can
+		 * affect the position of the bad block marker.
+		 */
+		/* Get the bad block marker */
+		badblock = readl(&src[CONFIG_SYS_NAND_BAD_BLOCK_POS / 4]);
+		badblock >>= 8 * (CONFIG_SYS_NAND_BAD_BLOCK_POS % 4);
+		badblock &= 0xff;
+
+		/* bad block marker verify */
+		if (badblock != 0xff)
+			return 1; /* potential bad block */
+	}
+
+	return 0;
+}
+
+static int nand_load(unsigned int from, unsigned int size, unsigned char *buf)
+{
+	int i;
+	unsigned int page;
+	unsigned int maxpages = CONFIG_SYS_NAND_SIZE /
+				CONFIG_SYS_NAND_PAGE_SIZE;
+
+	nfc_nand_init();
+
+	/* Convert to page number */
+	page = from / CONFIG_SYS_NAND_PAGE_SIZE;
+	i = 0;
+
+	while (i < size / CONFIG_SYS_NAND_PAGE_SIZE) {
+		if (nfc_read_page(page, buf) < 0)
+			return -1;
+
+		page++;
+		i++;
+		buf = buf + CONFIG_SYS_NAND_PAGE_SIZE;
+
+		/*
+		 * Check if we have crossed a block boundary, and if so
+		 * check for bad block.
+		 */
+		if (!(page % CONFIG_SYS_NAND_PAGE_COUNT)) {
+			/*
+			 * Yes, new block. See if this block is good. If not,
+			 * loop until we find a good block.
+			 */
+			while (is_badblock(page)) {
+				page = page + CONFIG_SYS_NAND_PAGE_COUNT;
+				/* Check i we've reached the end of flash. */
+				if (page >= maxpages)
+					return -1;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * The main entry for NAND booting. It's necessary that SDRAM is already
+ * configured and available since this code loads the main U-Boot image
+ * from NAND into SDRAM and starts it from there.
+ */
+void nand_boot(void)
+{
+	__attribute__((noreturn)) void (*uboot)(void);
+
+	/*
+	 * CONFIG_SYS_NAND_U_BOOT_OFFS and CONFIG_SYS_NAND_U_BOOT_SIZE must
+	 * be aligned to full pages
+	 */
+	if (!nand_load(CONFIG_SYS_NAND_U_BOOT_OFFS, CONFIG_SYS_NAND_U_BOOT_SIZE,
+		       (uchar *)CONFIG_SYS_NAND_U_BOOT_DST)) {
+		/* Copy from NAND successful, start U-boot */
+		uboot = (void *)CONFIG_SYS_NAND_U_BOOT_START;
+		uboot();
+	} else {
+		/* Unrecoverable error when copying from NAND */
+		hang();
+	}
+}
+
+/*
+ * Called in case of an exception.
+ */
+void hang(void)
+{
+	/* Loop forever */
+	while (1) ;
+}
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index 6ebbb5ebee..213d2c945a 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -156,7 +156,7 @@ static uint8_t ndfc_read_byte(struct mtd_info *mtd)
 
 	struct nand_chip *chip = mtd->priv;
 
-#ifdef CONFIG_SYS_NDFC_16BIT
+#ifdef CONFIG_SYS_NAND_BUSWIDTH_16BIT
 	return (uint8_t) readw(chip->IO_ADDR_R);
 #else
 	return readb(chip->IO_ADDR_R);
@@ -218,7 +218,7 @@ int board_nand_init(struct nand_chip *nand)
 	nand->ecc.bytes = 3;
 	nand->select_chip = ndfc_select_chip;
 
-#ifdef CONFIG_SYS_NDFC_16BIT
+#ifdef CONFIG_SYS_NAND_BUSWIDTH_16BIT
 	nand->options |= NAND_BUSWIDTH_16;
 #endif
 
diff --git a/drivers/mtd/nand/omap_gpmc.c b/drivers/mtd/nand/omap_gpmc.c
index bbf5443ec8..bc1bcad3ba 100644
--- a/drivers/mtd/nand/omap_gpmc.c
+++ b/drivers/mtd/nand/omap_gpmc.c
@@ -26,8 +26,9 @@
 #include <asm/errno.h>
 #include <asm/arch/mem.h>
 #include <asm/arch/cpu.h>
-#include <asm/arch/omap_gpmc.h>
+#include <asm/omap_gpmc.h>
 #include <linux/mtd/nand_ecc.h>
+#include <linux/bch.h>
 #include <linux/compiler.h>
 #include <nand.h>
 #ifdef CONFIG_AM33XX
@@ -37,6 +38,8 @@
 static uint8_t cs;
 static __maybe_unused struct nand_ecclayout hw_nand_oob =
 	GPMC_NAND_HW_ECC_LAYOUT;
+static __maybe_unused struct nand_ecclayout hw_bch8_nand_oob =
+	GPMC_NAND_HW_BCH8_ECC_LAYOUT;
 
 /*
  * omap_nand_hwcontrol - Set the address pointers corretly for the
@@ -239,13 +242,13 @@ static void __maybe_unused omap_enable_hwecc(struct mtd_info *mtd, int32_t mode)
 }
 
 /*
- * BCH8 support (needs ELM and thus AM33xx-only)
+ * Generic BCH interface
  */
-#ifdef CONFIG_AM33XX
 struct nand_bch_priv {
 	uint8_t mode;
 	uint8_t type;
 	uint8_t nibbles;
+	struct bch_control *control;
 };
 
 /* bch types */
@@ -253,21 +256,146 @@ struct nand_bch_priv {
 #define ECC_BCH8	1
 #define ECC_BCH16	2
 
+/* GPMC ecc engine settings */
+#define BCH_WRAPMODE_1		1	/* BCH wrap mode 1 */
+#define BCH_WRAPMODE_6		6	/* BCH wrap mode 6 */
+
 /* BCH nibbles for diff bch levels */
 #define NAND_ECC_HW_BCH ((uint8_t)(NAND_ECC_HW_OOB_FIRST) + 1)
 #define ECC_BCH4_NIBBLES	13
 #define ECC_BCH8_NIBBLES	26
 #define ECC_BCH16_NIBBLES	52
 
-static struct nand_ecclayout hw_bch8_nand_oob = GPMC_NAND_HW_BCH8_ECC_LAYOUT;
-
-static struct nand_bch_priv bch_priv = {
+/*
+ * This can be a single instance cause all current users have only one NAND
+ * with nearly the same setup (BCH8, some with ELM and others with sw BCH
+ * library).
+ * When some users with other BCH strength will exists this have to change!
+ */
+static __maybe_unused struct nand_bch_priv bch_priv = {
 	.mode = NAND_ECC_HW_BCH,
 	.type = ECC_BCH8,
-	.nibbles = ECC_BCH8_NIBBLES
+	.nibbles = ECC_BCH8_NIBBLES,
+	.control = NULL
 };
 
 /*
+ * omap_hwecc_init_bch - Initialize the BCH Hardware ECC for NAND flash in
+ *				GPMC controller
+ * @mtd:	MTD device structure
+ * @mode:	Read/Write mode
+ */
+__maybe_unused
+static void omap_hwecc_init_bch(struct nand_chip *chip, int32_t mode)
+{
+	uint32_t val;
+	uint32_t dev_width = (chip->options & NAND_BUSWIDTH_16) >> 1;
+#ifdef CONFIG_AM33XX
+	uint32_t unused_length = 0;
+#endif
+	uint32_t wr_mode = BCH_WRAPMODE_6;
+	struct nand_bch_priv *bch = chip->priv;
+
+	/* Clear the ecc result registers, select ecc reg as 1 */
+	writel(ECCCLEAR | ECCRESULTREG1, &gpmc_cfg->ecc_control);
+
+#ifdef CONFIG_AM33XX
+	wr_mode = BCH_WRAPMODE_1;
+
+	switch (bch->nibbles) {
+	case ECC_BCH4_NIBBLES:
+		unused_length = 3;
+		break;
+	case ECC_BCH8_NIBBLES:
+		unused_length = 2;
+		break;
+	case ECC_BCH16_NIBBLES:
+		unused_length = 0;
+		break;
+	}
+
+	/*
+	 * This is ecc_size_config for ELM mode.
+	 * Here we are using different settings for read and write access and
+	 * also depending on BCH strength.
+	 */
+	switch (mode) {
+	case NAND_ECC_WRITE:
+		/* write access only setup eccsize1 config */
+		val = ((unused_length + bch->nibbles) << 22);
+		break;
+
+	case NAND_ECC_READ:
+	default:
+		/*
+		 * by default eccsize0 selected for ecc1resultsize
+		 * eccsize0 config.
+		 */
+		val  = (bch->nibbles << 12);
+		/* eccsize1 config */
+		val |= (unused_length << 22);
+		break;
+	}
+#else
+	/*
+	 * This ecc_size_config setting is for BCH sw library.
+	 *
+	 * Note: we only support BCH8 currently with BCH sw library!
+	 * Should be really easy to adobt to BCH4, however some omap3 have
+	 * flaws with BCH4.
+	 *
+	 * Here we are using wrapping mode 6 both for reading and writing, with:
+	 *  size0 = 0  (no additional protected byte in spare area)
+	 *  size1 = 32 (skip 32 nibbles = 16 bytes per sector in spare area)
+	 */
+	val = (32 << 22) | (0 << 12);
+#endif
+	/* ecc size configuration */
+	writel(val, &gpmc_cfg->ecc_size_config);
+
+	/*
+	 * Configure the ecc engine in gpmc
+	 * We assume 512 Byte sector pages for access to NAND.
+	 */
+	val  = (1 << 16);		/* enable BCH mode */
+	val |= (bch->type << 12);	/* setup BCH type */
+	val |= (wr_mode << 8);		/* setup wrapping mode */
+	val |= (dev_width << 7);	/* setup device width (16 or 8 bit) */
+	val |= (cs << 1);		/* setup chip select to work on */
+	debug("set ECC_CONFIG=0x%08x\n", val);
+	writel(val, &gpmc_cfg->ecc_config);
+}
+
+/*
+ * omap_enable_ecc_bch - This function enables the bch h/w ecc functionality
+ * @mtd:	MTD device structure
+ * @mode:	Read/Write mode
+ */
+__maybe_unused
+static void omap_enable_ecc_bch(struct mtd_info *mtd, int32_t mode)
+{
+	struct nand_chip *chip = mtd->priv;
+
+	omap_hwecc_init_bch(chip, mode);
+	/* enable ecc */
+	writel((readl(&gpmc_cfg->ecc_config) | 0x1), &gpmc_cfg->ecc_config);
+}
+
+/*
+ * omap_ecc_disable - Disable H/W ECC calculation
+ *
+ * @mtd:	MTD device structure
+ */
+static void __maybe_unused omap_ecc_disable(struct mtd_info *mtd)
+{
+	writel((readl(&gpmc_cfg->ecc_config) & ~0x1), &gpmc_cfg->ecc_config);
+}
+
+/*
+ * BCH8 support (needs ELM and thus AM33xx-only)
+ */
+#ifdef CONFIG_AM33XX
+/*
  * omap_read_bch8_result - Read BCH result for BCH8 level
  *
  * @mtd:	MTD device structure
@@ -306,18 +434,6 @@ static void omap_read_bch8_result(struct mtd_info *mtd, uint8_t big_endian,
 }
 
 /*
- * omap_ecc_disable - Disable H/W ECC calculation
- *
- * @mtd:	MTD device structure
- *
- */
-static void omap_ecc_disable(struct mtd_info *mtd)
-{
-	writel((readl(&gpmc_cfg->ecc_config) & ~0x1),
-		&gpmc_cfg->ecc_config);
-}
-
-/*
  * omap_rotate_ecc_bch - Rotate the syndrome bytes
  *
  * @mtd:	MTD device structure
@@ -468,76 +584,6 @@ static int omap_correct_data_bch(struct mtd_info *mtd, uint8_t *dat,
 
 	return 0;
 }
-/*
- * omap_hwecc_init_bch - Initialize the BCH Hardware ECC for NAND flash in
- *				GPMC controller
- * @mtd:       MTD device structure
- * @mode:	Read/Write mode
- */
-static void omap_hwecc_init_bch(struct nand_chip *chip, int32_t mode)
-{
-	uint32_t val, dev_width = (chip->options & NAND_BUSWIDTH_16) >> 1;
-	uint32_t unused_length = 0;
-	struct nand_bch_priv *bch = chip->priv;
-
-	switch (bch->nibbles) {
-	case ECC_BCH4_NIBBLES:
-		unused_length = 3;
-		break;
-	case ECC_BCH8_NIBBLES:
-		unused_length = 2;
-		break;
-	case ECC_BCH16_NIBBLES:
-		unused_length = 0;
-		break;
-	}
-
-	/* Clear the ecc result registers, select ecc reg as 1 */
-	writel(ECCCLEAR | ECCRESULTREG1, &gpmc_cfg->ecc_control);
-
-	switch (mode) {
-	case NAND_ECC_WRITE:
-		/* eccsize1 config */
-		val = ((unused_length + bch->nibbles) << 22);
-		break;
-
-	case NAND_ECC_READ:
-	default:
-		/* by default eccsize0 selected for ecc1resultsize */
-		/* eccsize0 config */
-		val  = (bch->nibbles << 12);
-		/* eccsize1 config */
-		val |= (unused_length << 22);
-		break;
-	}
-	/* ecc size configuration */
-	writel(val, &gpmc_cfg->ecc_size_config);
-	/* by default 512bytes sector page is selected */
-	/* set bch mode */
-	val  = (1 << 16);
-	/* bch4 / bch8 / bch16 */
-	val |= (bch->type << 12);
-	/* set wrap mode to 1 */
-	val |= (1 << 8);
-	val |= (dev_width << 7);
-	val |= (cs << 1);
-	writel(val, &gpmc_cfg->ecc_config);
-}
-
-/*
- * omap_enable_ecc_bch- This function enables the bch h/w ecc functionality
- * @mtd:        MTD device structure
- * @mode:       Read/Write mode
- *
- */
-static void omap_enable_ecc_bch(struct mtd_info *mtd, int32_t mode)
-{
-	struct nand_chip *chip = mtd->priv;
-
-	omap_hwecc_init_bch(chip, mode);
-	/* enable ecc */
-	writel((readl(&gpmc_cfg->ecc_config) | 0x1), &gpmc_cfg->ecc_config);
-}
 
 /**
  * omap_read_page_bch - hardware ecc based page read function
@@ -602,15 +648,137 @@ static int omap_read_page_bch(struct mtd_info *mtd, struct nand_chip *chip,
 }
 #endif /* CONFIG_AM33XX */
 
-#ifndef CONFIG_SPL_BUILD
 /*
- * omap_nand_switch_ecc - switch the ECC operation b/w h/w ecc and s/w ecc.
- * The default is to come up on s/w ecc
+ * OMAP3 BCH8 support (with BCH library)
+ */
+#ifdef CONFIG_NAND_OMAP_BCH8
+/*
+ *  omap_calculate_ecc_bch - Read BCH ECC result
  *
- * @hardware - 1 -switch to h/w ecc, 0 - s/w ecc
+ *  @mtd:	MTD device structure
+ *  @dat:	The pointer to data on which ecc is computed (unused here)
+ *  @ecc:	The ECC output buffer
+ */
+static int omap_calculate_ecc_bch(struct mtd_info *mtd, const uint8_t *dat,
+				uint8_t *ecc)
+{
+	int ret = 0;
+	size_t i;
+	unsigned long nsectors, val1, val2, val3, val4;
+
+	nsectors = ((readl(&gpmc_cfg->ecc_config) >> 4) & 0x7) + 1;
+
+	for (i = 0; i < nsectors; i++) {
+		/* Read hw-computed remainder */
+		val1 = readl(&gpmc_cfg->bch_result_0_3[i].bch_result_x[0]);
+		val2 = readl(&gpmc_cfg->bch_result_0_3[i].bch_result_x[1]);
+		val3 = readl(&gpmc_cfg->bch_result_0_3[i].bch_result_x[2]);
+		val4 = readl(&gpmc_cfg->bch_result_0_3[i].bch_result_x[3]);
+
+		/*
+		 * Add constant polynomial to remainder, in order to get an ecc
+		 * sequence of 0xFFs for a buffer filled with 0xFFs.
+		 */
+		*ecc++ = 0xef ^ (val4 & 0xFF);
+		*ecc++ = 0x51 ^ ((val3 >> 24) & 0xFF);
+		*ecc++ = 0x2e ^ ((val3 >> 16) & 0xFF);
+		*ecc++ = 0x09 ^ ((val3 >> 8) & 0xFF);
+		*ecc++ = 0xed ^ (val3 & 0xFF);
+		*ecc++ = 0x93 ^ ((val2 >> 24) & 0xFF);
+		*ecc++ = 0x9a ^ ((val2 >> 16) & 0xFF);
+		*ecc++ = 0xc2 ^ ((val2 >> 8) & 0xFF);
+		*ecc++ = 0x97 ^ (val2 & 0xFF);
+		*ecc++ = 0x79 ^ ((val1 >> 24) & 0xFF);
+		*ecc++ = 0xe5 ^ ((val1 >> 16) & 0xFF);
+		*ecc++ = 0x24 ^ ((val1 >> 8) & 0xFF);
+		*ecc++ = 0xb5 ^ (val1 & 0xFF);
+	}
+
+	/*
+	 * Stop reading anymore ECC vals and clear old results
+	 * enable will be called if more reads are required
+	 */
+	omap_ecc_disable(mtd);
+
+	return ret;
+}
+
+/**
+ * omap_correct_data_bch - Decode received data and correct errors
+ * @mtd: MTD device structure
+ * @data: page data
+ * @read_ecc: ecc read from nand flash
+ * @calc_ecc: ecc read from HW ECC registers
+ */
+static int omap_correct_data_bch(struct mtd_info *mtd, u_char *data,
+				 u_char *read_ecc, u_char *calc_ecc)
+{
+	int i, count;
+	/* cannot correct more than 8 errors */
+	unsigned int errloc[8];
+	struct nand_chip *chip = mtd->priv;
+	struct nand_bch_priv *chip_priv = chip->priv;
+	struct bch_control *bch = chip_priv->control;
+
+	count = decode_bch(bch, NULL, 512, read_ecc, calc_ecc, NULL, errloc);
+	if (count > 0) {
+		/* correct errors */
+		for (i = 0; i < count; i++) {
+			/* correct data only, not ecc bytes */
+			if (errloc[i] < 8*512)
+				data[errloc[i]/8] ^= 1 << (errloc[i] & 7);
+			printf("corrected bitflip %u\n", errloc[i]);
+#ifdef DEBUG
+			puts("read_ecc: ");
+			/*
+			 * BCH8 have 13 bytes of ECC; BCH4 needs adoption
+			 * here!
+			 */
+			for (i = 0; i < 13; i++)
+				printf("%02x ", read_ecc[i]);
+			puts("\n");
+			puts("calc_ecc: ");
+			for (i = 0; i < 13; i++)
+				printf("%02x ", calc_ecc[i]);
+			puts("\n");
+#endif
+		}
+	} else if (count < 0) {
+		puts("ecc unrecoverable error\n");
+	}
+	return count;
+}
+
+/**
+ * omap_free_bch - Release BCH ecc resources
+ * @mtd: MTD device structure
+ */
+static void __maybe_unused omap_free_bch(struct mtd_info *mtd)
+{
+	struct nand_chip *chip = mtd->priv;
+	struct nand_bch_priv *chip_priv = chip->priv;
+	struct bch_control *bch = NULL;
+
+	if (chip_priv)
+		bch = chip_priv->control;
+
+	if (bch) {
+		free_bch(bch);
+		chip_priv->control = NULL;
+	}
+}
+#endif /* CONFIG_NAND_OMAP_BCH8 */
+
+#ifndef CONFIG_SPL_BUILD
+/*
+ * omap_nand_switch_ecc - switch the ECC operation between different engines
+ * (h/w and s/w) and different algorithms (hamming and BCHx)
  *
+ * @hardware		- true if one of the HW engines should be used
+ * @eccstrength		- the number of bits that could be corrected
+ *			  (1 - hamming, 4 - BCH4, 8 - BCH8, 16 - BCH16)
  */
-void omap_nand_switch_ecc(int32_t hardware)
+void omap_nand_switch_ecc(uint32_t hardware, uint32_t eccstrength)
 {
 	struct nand_chip *nand;
 	struct mtd_info *mtd;
@@ -628,6 +796,7 @@ void omap_nand_switch_ecc(int32_t hardware)
 	nand->options |= NAND_OWN_BUFFERS;
 
 	/* Reset ecc interface */
+	nand->ecc.mode = NAND_ECC_NONE;
 	nand->ecc.read_page = NULL;
 	nand->ecc.write_page = NULL;
 	nand->ecc.read_oob = NULL;
@@ -637,28 +806,35 @@ void omap_nand_switch_ecc(int32_t hardware)
 	nand->ecc.calculate = NULL;
 
 	/* Setup the ecc configurations again */
-	if (hardware == 1) {
-		nand->ecc.mode = NAND_ECC_HW;
-		nand->ecc.layout = &hw_nand_oob;
-		nand->ecc.size = 512;
-		nand->ecc.bytes = 3;
-		nand->ecc.hwctl = omap_enable_hwecc;
-		nand->ecc.correct = omap_correct_data;
-		nand->ecc.calculate = omap_calculate_ecc;
-		omap_hwecc_init(nand);
-		printf("HW ECC selected\n");
+	if (hardware) {
+		if (eccstrength == 1) {
+			nand->ecc.mode = NAND_ECC_HW;
+			nand->ecc.layout = &hw_nand_oob;
+			nand->ecc.size = 512;
+			nand->ecc.bytes = 3;
+			nand->ecc.hwctl = omap_enable_hwecc;
+			nand->ecc.correct = omap_correct_data;
+			nand->ecc.calculate = omap_calculate_ecc;
+			omap_hwecc_init(nand);
+			printf("1-bit hamming HW ECC selected\n");
+		}
+#if defined(CONFIG_AM33XX) || defined(CONFIG_NAND_OMAP_BCH8)
+		else if (eccstrength == 8) {
+			nand->ecc.mode = NAND_ECC_HW;
+			nand->ecc.layout = &hw_bch8_nand_oob;
+			nand->ecc.size = 512;
 #ifdef CONFIG_AM33XX
-	} else if (hardware == 2) {
-		nand->ecc.mode = NAND_ECC_HW;
-		nand->ecc.layout = &hw_bch8_nand_oob;
-		nand->ecc.size = 512;
-		nand->ecc.bytes = 14;
-		nand->ecc.read_page = omap_read_page_bch;
-		nand->ecc.hwctl = omap_enable_ecc_bch;
-		nand->ecc.correct = omap_correct_data_bch;
-		nand->ecc.calculate = omap_calculate_ecc_bch;
-		omap_hwecc_init_bch(nand, NAND_ECC_READ);
-		printf("HW BCH8 selected\n");
+			nand->ecc.bytes = 14;
+			nand->ecc.read_page = omap_read_page_bch;
+#else
+			nand->ecc.bytes = 13;
+#endif
+			nand->ecc.hwctl = omap_enable_ecc_bch;
+			nand->ecc.correct = omap_correct_data_bch;
+			nand->ecc.calculate = omap_calculate_ecc_bch;
+			omap_hwecc_init_bch(nand, NAND_ECC_READ);
+			printf("8-bit BCH HW ECC selected\n");
+		}
 #endif
 	} else {
 		nand->ecc.mode = NAND_ECC_SOFT;
@@ -732,16 +908,28 @@ int board_nand_init(struct nand_chip *nand)
 
 	nand->chip_delay = 100;
 
+#if defined(CONFIG_AM33XX) || defined(CONFIG_NAND_OMAP_BCH8)
 #ifdef CONFIG_AM33XX
+	/* AM33xx uses the ELM */
 	/* required in case of BCH */
 	elm_init();
-
+#else
+	/*
+	 * Whereas other OMAP based SoC do not have the ELM, they use the BCH
+	 * SW library.
+	 */
+	bch_priv.control = init_bch(13, 8, 0x201b /* hw polynominal */);
+	if (!bch_priv.control) {
+		puts("Could not init_bch()\n");
+		return -ENODEV;
+	}
+#endif
 	/* BCH info that will be correct for SPL or overridden otherwise. */
 	nand->priv = &bch_priv;
 #endif
 
 	/* Default ECC mode */
-#ifdef CONFIG_AM33XX
+#if defined(CONFIG_AM33XX) || defined(CONFIG_NAND_OMAP_BCH8)
 	nand->ecc.mode = NAND_ECC_HW;
 	nand->ecc.layout = &hw_bch8_nand_oob;
 	nand->ecc.size = CONFIG_SYS_NAND_ECCSIZE;
@@ -749,7 +937,9 @@ int board_nand_init(struct nand_chip *nand)
 	nand->ecc.hwctl = omap_enable_ecc_bch;
 	nand->ecc.correct = omap_correct_data_bch;
 	nand->ecc.calculate = omap_calculate_ecc_bch;
+#ifdef CONFIG_AM33XX
 	nand->ecc.read_page = omap_read_page_bch;
+#endif
 	omap_hwecc_init_bch(nand, NAND_ECC_READ);
 #else
 #if !defined(CONFIG_SPL_BUILD) || defined(CONFIG_SPL_NAND_SOFTECC)
diff --git a/drivers/mtd/nand/s3c64xx.c b/drivers/mtd/nand/s3c64xx.c
deleted file mode 100644
index 87f0341066..0000000000
--- a/drivers/mtd/nand/s3c64xx.c
+++ /dev/null
@@ -1,295 +0,0 @@
-/*
- * (C) Copyright 2006 DENX Software Engineering
- *
- * Implementation for U-Boot 1.1.6 by Samsung
- *
- * (C) Copyright 2008
- * Guennadi Liakhovetki, DENX Software Engineering, <lg@denx.de>
- *
- * See file CREDITS for list of people who contributed to this
- * project.
- *
- * 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 <common.h>
-
-#include <nand.h>
-#include <linux/mtd/nand.h>
-
-#include <asm/arch/s3c6400.h>
-
-#include <asm/io.h>
-#include <asm/errno.h>
-
-#define MAX_CHIPS	2
-static int nand_cs[MAX_CHIPS] = {0, 1};
-
-#ifdef CONFIG_NAND_SPL
-#define printf(arg...) do {} while (0)
-#endif
-
-/* Nand flash definition values by jsgood */
-#ifdef S3C_NAND_DEBUG
-/*
- * Function to print out oob buffer for debugging
- * Written by jsgood
- */
-static void print_oob(const char *header, struct mtd_info *mtd)
-{
-	int i;
-	struct nand_chip *chip = mtd->priv;
-
-	printf("%s:\t", header);
-
-	for (i = 0; i < 64; i++)
-		printf("%02x ", chip->oob_poi[i]);
-
-	printf("\n");
-}
-#endif /* S3C_NAND_DEBUG */
-
-static void s3c_nand_select_chip(struct mtd_info *mtd, int chip)
-{
-	int ctrl = readl(NFCONT);
-
-	switch (chip) {
-	case -1:
-		ctrl |= 6;
-		break;
-	case 0:
-		ctrl &= ~2;
-		break;
-	case 1:
-		ctrl &= ~4;
-		break;
-	default:
-		return;
-	}
-
-	writel(ctrl, NFCONT);
-}
-
-/*
- * Hardware specific access to control-lines function
- * Written by jsgood
- */
-static void s3c_nand_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
-{
-	struct nand_chip *this = mtd->priv;
-
-	if (ctrl & NAND_CTRL_CHANGE) {
-		if (ctrl & NAND_CLE)
-			this->IO_ADDR_W = (void __iomem *)NFCMMD;
-		else if (ctrl & NAND_ALE)
-			this->IO_ADDR_W = (void __iomem *)NFADDR;
-		else
-			this->IO_ADDR_W = (void __iomem *)NFDATA;
-		if (ctrl & NAND_NCE)
-			s3c_nand_select_chip(mtd, *(int *)this->priv);
-		else
-			s3c_nand_select_chip(mtd, -1);
-	}
-
-	if (cmd != NAND_CMD_NONE)
-		writeb(cmd, this->IO_ADDR_W);
-}
-
-/*
- * Function for checking device ready pin
- * Written by jsgood
- */
-static int s3c_nand_device_ready(struct mtd_info *mtdinfo)
-{
-	return !!(readl(NFSTAT) & NFSTAT_RnB);
-}
-
-#ifdef CONFIG_SYS_S3C_NAND_HWECC
-/*
- * This function is called before encoding ecc codes to ready ecc engine.
- * Written by jsgood
- */
-static void s3c_nand_enable_hwecc(struct mtd_info *mtd, int mode)
-{
-	u_long nfcont, nfconf;
-
-	/*
-	 * The original driver used 4-bit ECC for "new" MLC chips, i.e., for
-	 * those with non-zero ID[3][3:2], which anyway only holds for ST
-	 * (Numonyx) chips
-	 */
-	nfconf = readl(NFCONF) & ~NFCONF_ECC_4BIT;
-
-	writel(nfconf, NFCONF);
-
-	/* Initialize & unlock */
-	nfcont = readl(NFCONT);
-	nfcont |= NFCONT_INITECC;
-	nfcont &= ~NFCONT_MECCLOCK;
-
-	if (mode == NAND_ECC_WRITE)
-		nfcont |= NFCONT_ECC_ENC;
-	else if (mode == NAND_ECC_READ)
-		nfcont &= ~NFCONT_ECC_ENC;
-
-	writel(nfcont, NFCONT);
-}
-
-/*
- * This function is called immediately after encoding ecc codes.
- * This function returns encoded ecc codes.
- * Written by jsgood
- */
-static int s3c_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
-				  u_char *ecc_code)
-{
-	u_long nfcont, nfmecc0;
-
-	/* Lock */
-	nfcont = readl(NFCONT);
-	nfcont |= NFCONT_MECCLOCK;
-	writel(nfcont, NFCONT);
-
-	nfmecc0 = readl(NFMECC0);
-
-	ecc_code[0] = nfmecc0 & 0xff;
-	ecc_code[1] = (nfmecc0 >> 8) & 0xff;
-	ecc_code[2] = (nfmecc0 >> 16) & 0xff;
-	ecc_code[3] = (nfmecc0 >> 24) & 0xff;
-
-	return 0;
-}
-
-/*
- * This function determines whether read data is good or not.
- * If SLC, must write ecc codes to controller before reading status bit.
- * If MLC, status bit is already set, so only reading is needed.
- * If status bit is good, return 0.
- * If correctable errors occured, do that.
- * If uncorrectable errors occured, return -1.
- * Written by jsgood
- */
-static int s3c_nand_correct_data(struct mtd_info *mtd, u_char *dat,
-				 u_char *read_ecc, u_char *calc_ecc)
-{
-	int ret = -1;
-	u_long nfestat0, nfmeccdata0, nfmeccdata1, err_byte_addr;
-	u_char err_type, repaired;
-
-	/* SLC: Write ecc to compare */
-	nfmeccdata0 = (calc_ecc[1] << 16) | calc_ecc[0];
-	nfmeccdata1 = (calc_ecc[3] << 16) | calc_ecc[2];
-	writel(nfmeccdata0, NFMECCDATA0);
-	writel(nfmeccdata1, NFMECCDATA1);
-
-	/* Read ecc status */
-	nfestat0 = readl(NFESTAT0);
-	err_type = nfestat0 & 0x3;
-
-	switch (err_type) {
-	case 0: /* No error */
-		ret = 0;
-		break;
-
-	case 1:
-		/*
-		 * 1 bit error (Correctable)
-		 * (nfestat0 >> 7) & 0x7ff	:error byte number
-		 * (nfestat0 >> 4) & 0x7	:error bit number
-		 */
-		err_byte_addr = (nfestat0 >> 7) & 0x7ff;
-		repaired = dat[err_byte_addr] ^ (1 << ((nfestat0 >> 4) & 0x7));
-
-		printf("S3C NAND: 1 bit error detected at byte %ld. "
-		       "Correcting from 0x%02x to 0x%02x...OK\n",
-		       err_byte_addr, dat[err_byte_addr], repaired);
-
-		dat[err_byte_addr] = repaired;
-
-		ret = 1;
-		break;
-
-	case 2: /* Multiple error */
-	case 3: /* ECC area error */
-		printf("S3C NAND: ECC uncorrectable error detected. "
-		       "Not correctable.\n");
-		ret = -1;
-		break;
-	}
-
-	return ret;
-}
-#endif /* CONFIG_SYS_S3C_NAND_HWECC */
-
-/*
- * Board-specific NAND initialization. The following members of the
- * argument are board-specific (per include/linux/mtd/nand.h):
- * - IO_ADDR_R?: address to read the 8 I/O lines of the flash device
- * - IO_ADDR_W?: address to write the 8 I/O lines of the flash device
- * - hwcontrol: hardwarespecific function for accesing control-lines
- * - dev_ready: hardwarespecific function for  accesing device ready/busy line
- * - enable_hwecc?: function to enable (reset)  hardware ecc generator. Must
- *   only be provided if a hardware ECC is available
- * - eccmode: mode of ecc, see defines
- * - chip_delay: chip dependent delay for transfering data from array to
- *   read regs (tR)
- * - options: various chip options. They can partly be set to inform
- *   nand_scan about special functionality. See the defines for further
- *   explanation
- * Members with a "?" were not set in the merged testing-NAND branch,
- * so they are not set here either.
- */
-int board_nand_init(struct nand_chip *nand)
-{
-	static int chip_n;
-
-	if (chip_n >= MAX_CHIPS)
-		return -ENODEV;
-
-	NFCONT_REG = (NFCONT_REG & ~NFCONT_WP) | NFCONT_ENABLE | 0x6;
-
-	nand->IO_ADDR_R		= (void __iomem *)NFDATA;
-	nand->IO_ADDR_W		= (void __iomem *)NFDATA;
-	nand->cmd_ctrl		= s3c_nand_hwcontrol;
-	nand->dev_ready		= s3c_nand_device_ready;
-	nand->select_chip	= s3c_nand_select_chip;
-	nand->options		= 0;
-#ifdef CONFIG_NAND_SPL
-	nand->read_byte		= nand_read_byte;
-	nand->write_buf		= nand_write_buf;
-	nand->read_buf		= nand_read_buf;
-#endif
-
-#ifdef CONFIG_SYS_S3C_NAND_HWECC
-	nand->ecc.hwctl		= s3c_nand_enable_hwecc;
-	nand->ecc.calculate	= s3c_nand_calculate_ecc;
-	nand->ecc.correct	= s3c_nand_correct_data;
-
-	/*
-	 * If you get more than 1 NAND-chip with different page-sizes on the
-	 * board one day, it will get more complicated...
-	 */
-	nand->ecc.mode		= NAND_ECC_HW;
-	nand->ecc.size		= CONFIG_SYS_NAND_ECCSIZE;
-	nand->ecc.bytes		= CONFIG_SYS_NAND_ECCBYTES;
-#else
-	nand->ecc.mode		= NAND_ECC_SOFT;
-#endif /* ! CONFIG_SYS_S3C_NAND_HWECC */
-
-	nand->priv		= nand_cs + chip_n++;
-
-	return 0;
-}
diff --git a/drivers/mtd/onenand/onenand_base.c b/drivers/mtd/onenand/onenand_base.c
index 1a7b40eaa3..858e322743 100644
--- a/drivers/mtd/onenand/onenand_base.c
+++ b/drivers/mtd/onenand/onenand_base.c
@@ -632,10 +632,6 @@ static int onenand_check_bufferram(struct mtd_info *mtd, loff_t addr)
 	int blockpage, found = 0;
 	unsigned int i;
 
-#ifdef CONFIG_S3C64XX
-	return 0;
-#endif
-
 	if (ONENAND_IS_2PLANE(this))
 		blockpage = onenand_get_2x_blockpage(mtd, addr);
 	else
diff --git a/drivers/mtd/onenand/samsung.c b/drivers/mtd/onenand/samsung.c
index 0d94ea5b1f..5eb2b3a424 100644
--- a/drivers/mtd/onenand/samsung.c
+++ b/drivers/mtd/onenand/samsung.c
@@ -1,5 +1,5 @@
 /*
- * S3C64XX/S5PC100 OneNAND driver at U-Boot
+ * S5PC100 OneNAND driver at U-Boot
  *
  * Copyright (C) 2008-2009 Samsung Electronics
  * Kyungmin Park <kyungmin.park@samsung.com>
@@ -62,12 +62,7 @@ do {									\
 #define ONENAND_MAIN_SPARE_ACCESS	0x16
 #define ONENAND_PIPELINE_READ		0x4000
 
-#if defined(CONFIG_S3C64XX)
-#define MAP_00				(0x0 << 24)
-#define MAP_01				(0x1 << 24)
-#define MAP_10				(0x2 << 24)
-#define MAP_11				(0x3 << 24)
-#elif defined(CONFIG_S5P)
+#if defined(CONFIG_S5P)
 #define MAP_00				(0x0 << 26)
 #define MAP_01				(0x1 << 26)
 #define MAP_10				(0x2 << 26)
@@ -116,12 +111,7 @@ static void s3c_write_cmd(int value, unsigned int cmd)
  * return the buffer address on the memory device
  * It will be combined with CMD_MAP_XX
  */
-#if defined(CONFIG_S3C64XX)
-static unsigned int s3c_mem_addr(int fba, int fpa, int fsa)
-{
-	return (fba << 12) | (fpa << 6) | (fsa << 4);
-}
-#elif defined(CONFIG_S5P)
+#if defined(CONFIG_S5P)
 static unsigned int s3c_mem_addr(int fba, int fpa, int fsa)
 {
 	return (fba << 13) | (fpa << 7) | (fsa << 5);
@@ -550,45 +540,6 @@ static void s3c_onenand_unlock_all(struct mtd_info *mtd)
 	s3c_onenand_check_lock_status(mtd);
 }
 
-#ifdef CONFIG_S3C64XX
-static void s3c_set_width_regs(struct onenand_chip *this)
-{
-	int dev_id, density;
-	int fba, fpa, fsa;
-	int dbs_dfs;
-
-	dev_id = DEVICE_ID0_REG;
-
-	density = (dev_id >> ONENAND_DEVICE_DENSITY_SHIFT) & 0xf;
-	dbs_dfs = !!(dev_id & ONENAND_DEVICE_IS_DDP);
-
-	fba = density + 7;
-	if (dbs_dfs)
-		fba--;		/* Decrease the fba */
-	fpa = 6;
-	if (density >= ONENAND_DEVICE_DENSITY_512Mb)
-		fsa = 2;
-	else
-		fsa = 1;
-
-	DPRINTK("FBA %lu, FPA %lu, FSA %lu, DDP %lu",
-		FBA_WIDTH0_REG, FPA_WIDTH0_REG, FSA_WIDTH0_REG,
-		DDP_DEVICE_REG);
-
-	DPRINTK("mem_cfg0 0x%lx, sync mode %lu, "
-		"dev_page_size %lu, BURST LEN %lu",
-		MEM_CFG0_REG, SYNC_MODE_REG,
-		DEV_PAGE_SIZE_REG, BURST_LEN0_REG);
-
-	DEV_PAGE_SIZE_REG = 0x1;
-
-	FBA_WIDTH0_REG = fba;
-	FPA_WIDTH0_REG = fpa;
-	FSA_WIDTH0_REG = fsa;
-	DBS_DFS_WIDTH0_REG = dbs_dfs;
-}
-#endif
-
 int s5pc110_chip_probe(struct mtd_info *mtd)
 {
 	return 0;
@@ -620,10 +571,7 @@ void s3c_onenand_init(struct mtd_info *mtd)
 
 	onenand->mtd = mtd;
 
-#if defined(CONFIG_S3C64XX)
-	onenand->base = (void *)0x70100000;
-	onenand->ahb_addr = (void *)0x20000000;
-#elif defined(CONFIG_S5P)
+#if defined(CONFIG_S5P)
 	onenand->base = (void *)0xE7100000;
 	onenand->ahb_addr = (void *)0xB0000000;
 #endif
diff --git a/drivers/serial/Makefile b/drivers/serial/Makefile
index de3f471996..fbc4e97e98 100644
--- a/drivers/serial/Makefile
+++ b/drivers/serial/Makefile
@@ -35,7 +35,6 @@ COBJS-$(CONFIG_LPC32XX_HSUART) += lpc32xx_hsuart.o
 COBJS-$(CONFIG_MCFUART) += mcfuart.o
 COBJS-$(CONFIG_OPENCORES_YANU) += opencores_yanu.o
 COBJS-$(CONFIG_SYS_NS16550) += ns16550.o
-COBJS-$(CONFIG_S3C64XX) += s3c64xx.o
 COBJS-$(CONFIG_S5P) += serial_s5p.o
 COBJS-$(CONFIG_SYS_NS16550_SERIAL) += serial_ns16550.o
 COBJS-$(CONFIG_IMX_SERIAL) += serial_imx.o
diff --git a/drivers/serial/ns16550.c b/drivers/serial/ns16550.c
index ed4e6b3a1f..7f013ab33c 100644
--- a/drivers/serial/ns16550.c
+++ b/drivers/serial/ns16550.c
@@ -36,10 +36,24 @@
 
 void NS16550_init(NS16550_t com_port, int baud_divisor)
 {
-#if (!defined(CONFIG_SYS_NS16550_BROKEN_TEMT))
+#if (defined(CONFIG_SPL_BUILD) && defined(CONFIG_OMAP34XX))
+	/*
+	 * On some OMAP3 devices when UART3 is configured for boot mode before
+	 * SPL starts only THRE bit is set. We have to empty the transmitter
+	 * before initialization starts.
+	 */
+	if ((serial_in(&com_port->lsr) & (UART_LSR_TEMT | UART_LSR_THRE))
+	     == UART_LSR_THRE) {
+		serial_out(UART_LCR_DLAB, &com_port->lcr);
+		serial_out(baud_divisor & 0xff, &com_port->dll);
+		serial_out((baud_divisor >> 8) & 0xff, &com_port->dlm);
+		serial_out(UART_LCRVAL, &com_port->lcr);
+		serial_out(0, &com_port->mdr1);
+	}
+#endif
+
 	while (!(serial_in(&com_port->lsr) & UART_LSR_TEMT))
 		;
-#endif
 
 	serial_out(CONFIG_SYS_NS16550_IER, &com_port->ier);
 #if (defined(CONFIG_OMAP) && !defined(CONFIG_OMAP3_ZOOM2)) || \
diff --git a/drivers/serial/s3c64xx.c b/drivers/serial/s3c64xx.c
deleted file mode 100644
index b590992dc8..0000000000
--- a/drivers/serial/s3c64xx.c
+++ /dev/null
@@ -1,187 +0,0 @@
-/*
- * (C) Copyright 2002
- * Gary Jennejohn, DENX Software Engineering, <garyj@denx.de>
- *
- * (C) Copyright 2008
- * Guennadi Liakhovetki, DENX Software Engineering, <lg@denx.de>
- *
- * 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 <common.h>
-#include <linux/compiler.h>
-#include <serial.h>
-#include <asm/arch/s3c6400.h>
-
-DECLARE_GLOBAL_DATA_PTR;
-
-#ifdef CONFIG_SERIAL1
-#define UART_NR	S3C64XX_UART0
-
-#elif defined(CONFIG_SERIAL2)
-#define UART_NR	S3C64XX_UART1
-
-#elif defined(CONFIG_SERIAL3)
-#define UART_NR	S3C64XX_UART2
-
-#else
-#error "Bad: you didn't configure serial ..."
-#endif
-
-/*
- * The coefficient, used to calculate the baudrate on S3C6400 UARTs is
- * calculated as
- * C = UBRDIV * 16 + number_of_set_bits_in_UDIVSLOT
- * however, section 31.6.11 of the datasheet doesn't recomment using 1 for 1,
- * 3 for 2, ... (2^n - 1) for n, instead, they suggest using these constants:
- */
-static const int udivslot[] = {
-	0,
-	0x0080,
-	0x0808,
-	0x0888,
-	0x2222,
-	0x4924,
-	0x4a52,
-	0x54aa,
-	0x5555,
-	0xd555,
-	0xd5d5,
-	0xddd5,
-	0xdddd,
-	0xdfdd,
-	0xdfdf,
-	0xffdf,
-};
-
-static void s3c64xx_serial_setbrg(void)
-{
-	s3c64xx_uart *const uart = s3c64xx_get_base_uart(UART_NR);
-	u32 pclk = get_PCLK();
-	u32 baudrate = gd->baudrate;
-	int i;
-
-	i = (pclk / baudrate) % 16;
-
-	uart->UBRDIV = pclk / baudrate / 16 - 1;
-	uart->UDIVSLOT = udivslot[i];
-
-	for (i = 0; i < 100; i++)
-		barrier();
-}
-
-/*
- * Initialise the serial port with the given baudrate. The settings
- * are always 8 data bits, no parity, 1 stop bit, no start bits.
- */
-static int s3c64xx_serial_init(void)
-{
-	s3c64xx_uart *const uart = s3c64xx_get_base_uart(UART_NR);
-
-	/* reset and enable FIFOs, set triggers to the maximum */
-	uart->UFCON = 0xff;
-	uart->UMCON = 0;
-	/* 8N1 */
-	uart->ULCON = 3;
-	/* No interrupts, no DMA, pure polling */
-	uart->UCON = 5;
-
-	serial_setbrg();
-
-	return 0;
-}
-
-/*
- * Read a single byte from the serial port. Returns 1 on success, 0
- * otherwise. When the function is succesfull, the character read is
- * written into its argument c.
- */
-static int s3c64xx_serial_getc(void)
-{
-	s3c64xx_uart *const uart = s3c64xx_get_base_uart(UART_NR);
-
-	/* wait for character to arrive */
-	while (!(uart->UTRSTAT & 0x1));
-
-	return uart->URXH & 0xff;
-}
-
-#ifdef CONFIG_MODEM_SUPPORT
-static int be_quiet;
-void disable_putc(void)
-{
-	be_quiet = 1;
-}
-
-void enable_putc(void)
-{
-	be_quiet = 0;
-}
-#endif
-
-
-/*
- * Output a single byte to the serial port.
- */
-static void s3c64xx_serial_putc(const char c)
-{
-	s3c64xx_uart *const uart = s3c64xx_get_base_uart(UART_NR);
-
-#ifdef CONFIG_MODEM_SUPPORT
-	if (be_quiet)
-		return;
-#endif
-
-	/* wait for room in the tx FIFO */
-	while (!(uart->UTRSTAT & 0x2));
-
-	uart->UTXH = c;
-
-	/* If \n, also do \r */
-	if (c == '\n')
-		serial_putc('\r');
-}
-
-/*
- * Test whether a character is in the RX buffer
- */
-static int s3c64xx_serial_tstc(void)
-{
-	s3c64xx_uart *const uart = s3c64xx_get_base_uart(UART_NR);
-
-	return uart->UTRSTAT & 0x1;
-}
-
-static struct serial_device s3c64xx_serial_drv = {
-	.name	= "s3c64xx_serial",
-	.start	= s3c64xx_serial_init,
-	.stop	= NULL,
-	.setbrg	= s3c64xx_serial_setbrg,
-	.putc	= s3c64xx_serial_putc,
-	.puts	= default_serial_puts,
-	.getc	= s3c64xx_serial_getc,
-	.tstc	= s3c64xx_serial_tstc,
-};
-
-void s3c64xx_serial_initialize(void)
-{
-	serial_register(&s3c64xx_serial_drv);
-}
-
-__weak struct serial_device *default_serial_console(void)
-{
-	return &s3c64xx_serial_drv;
-}
diff --git a/drivers/serial/serial.c b/drivers/serial/serial.c
index 7922bf0669..9f04643551 100644
--- a/drivers/serial/serial.c
+++ b/drivers/serial/serial.c
@@ -165,7 +165,6 @@ serial_initfunc(atmel_serial_initialize);
 serial_initfunc(lpc32xx_serial_initialize);
 serial_initfunc(mcf_serial_initialize);
 serial_initfunc(oc_serial_initialize);
-serial_initfunc(s3c64xx_serial_initialize);
 serial_initfunc(sandbox_serial_initialize);
 serial_initfunc(clps7111_serial_initialize);
 serial_initfunc(imx_serial_initialize);
@@ -259,7 +258,6 @@ void serial_initialize(void)
 	lpc32xx_serial_initialize();
 	mcf_serial_initialize();
 	oc_serial_initialize();
-	s3c64xx_serial_initialize();
 	sandbox_serial_initialize();
 	clps7111_serial_initialize();
 	imx_serial_initialize();
diff --git a/drivers/spi/exynos_spi.c b/drivers/spi/exynos_spi.c
index 51b3d30538..607e1cdec2 100644
--- a/drivers/spi/exynos_spi.c
+++ b/drivers/spi/exynos_spi.c
@@ -358,6 +358,7 @@ static inline struct exynos_spi *get_spi_base(int dev_index)
  * @param bus   SPI bus structure to fill with information
  * @return 0 if ok, or -FDT_ERR_NOTFOUND if something was missing
  */
+#ifdef CONFIG_OF_CONTROL
 static int spi_get_config(const void *blob, int node, struct spi_bus *bus)
 {
 	bus->node = node;
@@ -413,6 +414,7 @@ static int process_nodes(const void *blob, int node_list[], int count)
 
 	return 0;
 }
+#endif
 
 /* Sadly there is no error return from this function */
 void spi_init(void)
diff --git a/drivers/spi/mxc_spi.c b/drivers/spi/mxc_spi.c
index d792d8d493..5bed858787 100644
--- a/drivers/spi/mxc_spi.c
+++ b/drivers/spi/mxc_spi.c
@@ -137,11 +137,15 @@ static s32 spi_cfg_mxc(struct mxc_spi_slave *mxcs, unsigned int cs,
 		return -1;
 	}
 
-	reg_ctrl = reg_read(&regs->ctrl);
-
-	/* Reset spi */
-	reg_write(&regs->ctrl, (reg_ctrl & ~MXC_CSPICTRL_EN));
-	reg_write(&regs->ctrl, (reg_ctrl | MXC_CSPICTRL_EN));
+	/*
+	 * Reset SPI and set all CSs to master mode, if toggling
+	 * between slave and master mode we might see a glitch
+	 * on the clock line
+	 */
+	reg_ctrl = MXC_CSPICTRL_MODE_MASK;
+	reg_write(&regs->ctrl, reg_ctrl);
+	reg_ctrl |=  MXC_CSPICTRL_EN;
+	reg_write(&regs->ctrl, reg_ctrl);
 
 	/*
 	 * The following computation is taken directly from Freescale's code.
@@ -174,9 +178,6 @@ static s32 spi_cfg_mxc(struct mxc_spi_slave *mxcs, unsigned int cs,
 	reg_ctrl = (reg_ctrl & ~MXC_CSPICTRL_POSTDIV(0x0F)) |
 		MXC_CSPICTRL_POSTDIV(post_div);
 
-	/* always set to master mode */
-	reg_ctrl |= 1 << (cs + 4);
-
 	/* We need to disable SPI before changing registers */
 	reg_ctrl &= ~MXC_CSPICTRL_EN;
 
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 9a6f982080..87a59704db 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -31,7 +31,6 @@ COBJS-$(CONFIG_USB_ATMEL) += ohci-at91.o
 COBJS-$(CONFIG_USB_OHCI_DA8XX) += ohci-da8xx.o
 COBJS-$(CONFIG_USB_ISP116X_HCD) += isp116x-hcd.o
 COBJS-$(CONFIG_USB_R8A66597_HCD) += r8a66597-hcd.o
-COBJS-$(CONFIG_USB_S3C64XX) += s3c64xx-hcd.o
 COBJS-$(CONFIG_USB_SL811HS) += sl811-hcd.o
 COBJS-$(CONFIG_USB_OHCI_S3C24XX) += ohci-s3c24xx.o
 
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index bdbe250b01..bc17b85db5 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -66,7 +66,6 @@
 
 #if defined(CONFIG_ARM920T) || \
     defined(CONFIG_S3C24X0) || \
-    defined(CONFIG_S3C6400) || \
     defined(CONFIG_440EP) || \
     defined(CONFIG_PCI_OHCI) || \
     defined(CONFIG_MPC5200) || \
diff --git a/drivers/usb/host/s3c64xx-hcd.c b/drivers/usb/host/s3c64xx-hcd.c
deleted file mode 100644
index cd295dabb7..0000000000
--- a/drivers/usb/host/s3c64xx-hcd.c
+++ /dev/null
@@ -1,45 +0,0 @@
-/*
- * URB OHCI HCD (Host Controller Driver) initialization for USB on the S3C64XX.
- *
- * Copyright (C) 2008,
- * Guennadi Liakhovetski, DENX Software Engineering <lg@denx.de>
- *
- * See file CREDITS for list of people who contributed to this
- * project.
- *
- * 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 <common.h>
-#include <asm/arch/s3c6400.h>
-
-int usb_cpu_init(void)
-{
-	OTHERS_REG |= 0x10000;
-	return 0;
-}
-
-int usb_cpu_stop(void)
-{
-	OTHERS_REG &= ~0x10000;
-	return 0;
-}
-
-void usb_cpu_init_fail(void)
-{
-	OTHERS_REG &= ~0x10000;
-}
diff --git a/drivers/usb/musb-new/musb_core.c b/drivers/usb/musb-new/musb_core.c
index 040a5c0f0f..da9357149b 100644
--- a/drivers/usb/musb-new/musb_core.c
+++ b/drivers/usb/musb-new/musb_core.c
@@ -943,7 +943,9 @@ void musb_start(struct musb *musb)
 
 	/* put into basic highspeed mode and start session */
 	musb_writeb(regs, MUSB_POWER, MUSB_POWER_ISOUPDATE
+#ifdef CONFIG_USB_GADGET_DUALSPEED
 						| MUSB_POWER_HSENAB
+#endif
 						/* ENSUSPEND wedges tusb */
 						/* | MUSB_POWER_ENSUSPEND */
 						);
@@ -1421,6 +1423,7 @@ static int __devinit musb_core_init(u16 musb_type, struct musb *musb)
 		strcat(aInfo, ", dyn FIFOs");
 		musb->dyn_fifo = true;
 	}
+#ifndef CONFIG_MUSB_DISABLE_BULK_COMBINE_SPLIT
 	if (reg & MUSB_CONFIGDATA_MPRXE) {
 		strcat(aInfo, ", bulk combine");
 		musb->bulk_combine = true;
@@ -1429,6 +1432,10 @@ static int __devinit musb_core_init(u16 musb_type, struct musb *musb)
 		strcat(aInfo, ", bulk split");
 		musb->bulk_split = true;
 	}
+#else
+	musb->bulk_combine = false;
+	musb->bulk_split = false;
+#endif
 	if (reg & MUSB_CONFIGDATA_HBRXE) {
 		strcat(aInfo, ", HB-ISO Rx");
 		musb->hb_iso_rx = true;
diff --git a/drivers/usb/musb-new/musb_uboot.c b/drivers/usb/musb-new/musb_uboot.c
index 762cbc11df..15d2ec0078 100644
--- a/drivers/usb/musb-new/musb_uboot.c
+++ b/drivers/usb/musb-new/musb_uboot.c
@@ -174,7 +174,7 @@ int usb_gadget_register_driver(struct usb_gadget_driver *driver)
 {
 	int ret;
 
-	if (!driver || driver->speed < USB_SPEED_HIGH || !driver->bind ||
+	if (!driver || driver->speed < USB_SPEED_FULL || !driver->bind ||
 	    !driver->setup) {
 		printf("bad parameter.\n");
 		return -EINVAL;
diff --git a/drivers/video/exynos_dp.c b/drivers/video/exynos_dp.c
index d72fa565a7..87bb90777e 100644
--- a/drivers/video/exynos_dp.c
+++ b/drivers/video/exynos_dp.c
@@ -27,11 +27,21 @@
 #include <asm/arch/cpu.h>
 #include <asm/arch/dp_info.h>
 #include <asm/arch/dp.h>
+#include <fdtdec.h>
+#include <libfdt.h>
 
 #include "exynos_dp_lowlevel.h"
 
+DECLARE_GLOBAL_DATA_PTR;
+
 static struct exynos_dp_platform_data *dp_pd;
 
+void __exynos_set_dp_phy(unsigned int onoff)
+{
+}
+void exynos_set_dp_phy(unsigned int onoff)
+	__attribute__((weak, alias("__exynos_set_dp_phy")));
+
 static void exynos_dp_disp_info(struct edp_disp_info *disp_info)
 {
 	disp_info->h_total = disp_info->h_res + disp_info->h_sync_width +
@@ -853,6 +863,62 @@ static unsigned int exynos_dp_config_video(struct edp_device_info *edp_info)
 	return ret;
 }
 
+#ifdef CONFIG_OF_CONTROL
+int exynos_dp_parse_dt(const void *blob, struct edp_device_info *edp_info)
+{
+	unsigned int node = fdtdec_next_compatible(blob, 0,
+						COMPAT_SAMSUNG_EXYNOS5_DP);
+	if (node <= 0) {
+		debug("exynos_dp: Can't get device node for dp\n");
+		return -ENODEV;
+	}
+
+	edp_info->disp_info.h_res = fdtdec_get_int(blob, node,
+							"samsung,h-res", 0);
+	edp_info->disp_info.h_sync_width = fdtdec_get_int(blob, node,
+						"samsung,h-sync-width", 0);
+	edp_info->disp_info.h_back_porch = fdtdec_get_int(blob, node,
+						"samsung,h-back-porch", 0);
+	edp_info->disp_info.h_front_porch = fdtdec_get_int(blob, node,
+						"samsung,h-front-porch", 0);
+	edp_info->disp_info.v_res = fdtdec_get_int(blob, node,
+						"samsung,v-res", 0);
+	edp_info->disp_info.v_sync_width = fdtdec_get_int(blob, node,
+						"samsung,v-sync-width", 0);
+	edp_info->disp_info.v_back_porch = fdtdec_get_int(blob, node,
+						"samsung,v-back-porch", 0);
+	edp_info->disp_info.v_front_porch = fdtdec_get_int(blob, node,
+						"samsung,v-front-porch", 0);
+	edp_info->disp_info.v_sync_rate = fdtdec_get_int(blob, node,
+						"samsung,v-sync-rate", 0);
+
+	edp_info->lt_info.lt_status = fdtdec_get_int(blob, node,
+						"samsung,lt-status", 0);
+
+	edp_info->video_info.master_mode = fdtdec_get_int(blob, node,
+						"samsung,master-mode", 0);
+	edp_info->video_info.bist_mode = fdtdec_get_int(blob, node,
+						"samsung,bist-mode", 0);
+	edp_info->video_info.bist_pattern = fdtdec_get_int(blob, node,
+						"samsung,bist-pattern", 0);
+	edp_info->video_info.h_sync_polarity = fdtdec_get_int(blob, node,
+						"samsung,h-sync-polarity", 0);
+	edp_info->video_info.v_sync_polarity = fdtdec_get_int(blob, node,
+						"samsung,v-sync-polarity", 0);
+	edp_info->video_info.interlaced = fdtdec_get_int(blob, node,
+						"samsung,interlaced", 0);
+	edp_info->video_info.color_space = fdtdec_get_int(blob, node,
+						"samsung,color-space", 0);
+	edp_info->video_info.dynamic_range = fdtdec_get_int(blob, node,
+						"samsung,dynamic-range", 0);
+	edp_info->video_info.ycbcr_coeff = fdtdec_get_int(blob, node,
+						"samsung,ycbcr-coeff", 0);
+	edp_info->video_info.color_depth = fdtdec_get_int(blob, node,
+						"samsung,color-depth", 0);
+	return 0;
+}
+#endif
+
 unsigned int exynos_init_dp(void)
 {
 	unsigned int ret;
@@ -864,16 +930,22 @@ unsigned int exynos_init_dp(void)
 		return -EFAULT;
 	}
 
+#ifdef CONFIG_OF_CONTROL
+	if (exynos_dp_parse_dt(gd->fdt_blob, edp_info))
+		debug("unable to parse DP DT node\n");
+#else
 	edp_info = dp_pd->edp_dev_info;
 	if (edp_info == NULL) {
 		debug("failed to get edp_info data.\n");
 		return -EFAULT;
 	}
+#endif
+
+	exynos_dp_set_base_addr();
 
 	exynos_dp_disp_info(&edp_info->disp_info);
 
-	if (dp_pd->phy_enable)
-		dp_pd->phy_enable(1);
+	exynos_set_dp_phy(1);
 
 	ret = exynos_dp_init_dp();
 	if (ret != EXYNOS_DP_SUCCESS) {
diff --git a/drivers/video/exynos_dp_lowlevel.c b/drivers/video/exynos_dp_lowlevel.c
index 7b54c8084c..748d9b8b37 100644
--- a/drivers/video/exynos_dp_lowlevel.c
+++ b/drivers/video/exynos_dp_lowlevel.c
@@ -25,11 +25,34 @@
 #include <asm/arch/cpu.h>
 #include <asm/arch/dp_info.h>
 #include <asm/arch/dp.h>
+#include <fdtdec.h>
+#include <libfdt.h>
+
+/* Declare global data pointer */
+DECLARE_GLOBAL_DATA_PTR;
+
+struct exynos_dp *dp_regs;
+
+void exynos_dp_set_base_addr(void)
+{
+#ifdef CONFIG_OF_CONTROL
+	unsigned int node = fdtdec_next_compatible(gd->fdt_blob,
+					0, COMPAT_SAMSUNG_EXYNOS5_DP);
+	if (node <= 0)
+		debug("exynos_dp: Can't get device node for dp\n");
+
+	dp_regs = (struct exynos_dp *)fdtdec_get_addr(gd->fdt_blob,
+								node, "reg");
+	if (dp_regs == NULL)
+		debug("Can't get the DP base address\n");
+#else
+	dp_regs = (struct exynos_dp *)samsung_get_base_dp();
+#endif
+}
 
 static void exynos_dp_enable_video_input(unsigned int enable)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->video_ctl1);
 	reg &= ~VIDEO_EN_MASK;
@@ -47,7 +70,6 @@ void exynos_dp_enable_video_bist(unsigned int enable)
 {
 	/*enable video bist*/
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->video_ctl4);
 	reg &= ~VIDEO_BIST_MASK;
@@ -64,7 +86,6 @@ void exynos_dp_enable_video_bist(unsigned int enable)
 void exynos_dp_enable_video_mute(unsigned int enable)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->video_ctl1);
 	reg &= ~(VIDEO_MUTE_MASK);
@@ -80,7 +101,6 @@ void exynos_dp_enable_video_mute(unsigned int enable)
 static void exynos_dp_init_analog_param(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/*
 	 * Set termination
@@ -129,7 +149,6 @@ static void exynos_dp_init_analog_param(void)
 
 static void exynos_dp_init_interrupt(void)
 {
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 	/* Set interrupt registers to initial states */
 
 	/*
@@ -158,7 +177,6 @@ static void exynos_dp_init_interrupt(void)
 void exynos_dp_reset(void)
 {
 	unsigned int reg_func_1;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/*dp tx sw reset*/
 	writel(RESET_DP_TX, &dp_regs->tx_sw_reset);
@@ -186,7 +204,6 @@ void exynos_dp_reset(void)
 void exynos_dp_enable_sw_func(unsigned int enable)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->func_en1);
 	reg &= ~(SW_FUNC_EN_N);
@@ -202,7 +219,6 @@ void exynos_dp_enable_sw_func(unsigned int enable)
 unsigned int exynos_dp_set_analog_power_down(unsigned int block, u32 enable)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->phy_pd);
 	switch (block) {
@@ -256,7 +272,6 @@ unsigned int exynos_dp_set_analog_power_down(unsigned int block, u32 enable)
 unsigned int exynos_dp_get_pll_lock_status(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->debug_ctl);
 
@@ -269,7 +284,6 @@ unsigned int exynos_dp_get_pll_lock_status(void)
 static void exynos_dp_set_pll_power(unsigned int enable)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->pll_ctl);
 	reg &= ~(DP_PLL_PD);
@@ -285,7 +299,6 @@ int exynos_dp_init_analog_func(void)
 	int ret = EXYNOS_DP_SUCCESS;
 	unsigned int retry_cnt = 10;
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/*Power On All Analog block */
 	exynos_dp_set_analog_power_down(POWER_ALL, DP_DISABLE);
@@ -335,7 +348,6 @@ int exynos_dp_init_analog_func(void)
 void exynos_dp_init_hpd(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Clear interrupts releated to Hot Plug Dectect */
 	reg = HOTPLUG_CHG | HPD_LOST | PLUG;
@@ -354,7 +366,6 @@ void exynos_dp_init_hpd(void)
 static inline void exynos_dp_reset_aux(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Disable AUX channel module */
 	reg = readl(&dp_regs->func_en2);
@@ -367,7 +378,6 @@ static inline void exynos_dp_reset_aux(void)
 void exynos_dp_init_aux(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Clear inerrupts related to AUX channel */
 	reg = RPLY_RECEIV | AUX_ERR;
@@ -395,7 +405,6 @@ void exynos_dp_init_aux(void)
 void exynos_dp_config_interrupt(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* 0: mask, 1: unmask */
 	reg = COMMON_INT_MASK_1;
@@ -419,7 +428,6 @@ void exynos_dp_config_interrupt(void)
 unsigned int exynos_dp_get_plug_in_status(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->sys_ctl3);
 	if (reg & HPD_STATUS)
@@ -449,7 +457,6 @@ unsigned int exynos_dp_start_aux_transaction(void)
 	unsigned int reg;
 	unsigned int ret = 0;
 	unsigned int retry_cnt;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Enable AUX CH operation */
 	reg = readl(&dp_regs->aux_ch_ctl2);
@@ -498,7 +505,6 @@ unsigned int exynos_dp_write_byte_to_dpcd(unsigned int reg_addr,
 				unsigned char data)
 {
 	unsigned int reg, ret;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Clear AUX CH data buffer */
 	reg = BUF_CLR;
@@ -539,7 +545,6 @@ unsigned int exynos_dp_read_byte_from_dpcd(unsigned int reg_addr,
 {
 	unsigned int reg;
 	int retval;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Clear AUX CH data buffer */
 	reg = BUF_CLR;
@@ -583,7 +588,6 @@ unsigned int exynos_dp_write_bytes_to_dpcd(unsigned int reg_addr,
 	unsigned int cur_data_idx;
 	unsigned int retry_cnt;
 	unsigned int ret = 0;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Clear AUX CH data buffer */
 	reg = BUF_CLR;
@@ -649,7 +653,6 @@ unsigned int exynos_dp_read_bytes_from_dpcd(unsigned int reg_addr,
 	unsigned int cur_data_idx;
 	unsigned int retry_cnt;
 	unsigned int ret = 0;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Clear AUX CH data buffer */
 	reg = BUF_CLR;
@@ -711,7 +714,6 @@ int exynos_dp_select_i2c_device(unsigned int device_addr,
 {
 	unsigned int reg;
 	int retval;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Set EDID device address */
 	reg = device_addr;
@@ -746,7 +748,6 @@ int exynos_dp_read_byte_from_i2c(unsigned int device_addr,
 	unsigned int reg;
 	int i;
 	int retval;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	for (i = 0; i < 10; i++) {
 		/* Clear AUX CH data buffer */
@@ -790,7 +791,6 @@ int exynos_dp_read_bytes_from_i2c(unsigned int device_addr,
 	unsigned int cur_data_idx;
 	unsigned int defer = 0;
 	int retval = 0;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	for (i = 0; i < count; i += 16) { /* use 16 burst */
 		for (j = 0; j < 100; j++) {
@@ -854,7 +854,6 @@ int exynos_dp_read_bytes_from_i2c(unsigned int device_addr,
 void exynos_dp_reset_macro(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->phy_test);
 	reg |= MACRO_RST;
@@ -870,7 +869,6 @@ void exynos_dp_reset_macro(void)
 void exynos_dp_set_link_bandwidth(unsigned char bwtype)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = (unsigned int)bwtype;
 
@@ -883,7 +881,6 @@ unsigned char exynos_dp_get_link_bandwidth(void)
 {
 	unsigned char ret;
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->link_bw_set);
 	ret = (unsigned char)reg;
@@ -894,7 +891,6 @@ unsigned char exynos_dp_get_link_bandwidth(void)
 void exynos_dp_set_lane_count(unsigned char count)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = (unsigned int)count;
 
@@ -906,7 +902,6 @@ void exynos_dp_set_lane_count(unsigned char count)
 unsigned int exynos_dp_get_lane_count(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->lane_count_set);
 
@@ -915,7 +910,6 @@ unsigned int exynos_dp_get_lane_count(void)
 
 unsigned char exynos_dp_get_lanex_pre_emphasis(unsigned char lanecnt)
 {
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 	unsigned int reg_list[DP_LANE_CNT_4] = {
 		(unsigned int)&dp_regs->ln0_link_training_ctl,
 		(unsigned int)&dp_regs->ln1_link_training_ctl,
@@ -929,7 +923,6 @@ unsigned char exynos_dp_get_lanex_pre_emphasis(unsigned char lanecnt)
 void exynos_dp_set_lanex_pre_emphasis(unsigned char request_val,
 		unsigned char lanecnt)
 {
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 	unsigned int reg_list[DP_LANE_CNT_4] = {
 		(unsigned int)&dp_regs->ln0_link_training_ctl,
 		(unsigned int)&dp_regs->ln1_link_training_ctl,
@@ -944,7 +937,6 @@ void exynos_dp_set_lane_pre_emphasis(unsigned int level, unsigned char lanecnt)
 {
 	unsigned char i;
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 	unsigned int reg_list[DP_LANE_CNT_4] = {
 		(unsigned int)&dp_regs->ln0_link_training_ctl,
 		(unsigned int)&dp_regs->ln1_link_training_ctl,
@@ -967,7 +959,6 @@ void exynos_dp_set_lane_pre_emphasis(unsigned int level, unsigned char lanecnt)
 void exynos_dp_set_training_pattern(unsigned int pattern)
 {
 	unsigned int reg = 0;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	switch (pattern) {
 	case PRBS7:
@@ -996,7 +987,6 @@ void exynos_dp_set_training_pattern(unsigned int pattern)
 void exynos_dp_enable_enhanced_mode(unsigned char enable)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->sys_ctl4);
 	reg &= ~ENHANCED;
@@ -1010,7 +1000,6 @@ void exynos_dp_enable_enhanced_mode(unsigned char enable)
 void exynos_dp_enable_scrambling(unsigned int enable)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->training_ptn_set);
 	reg &= ~(SCRAMBLING_DISABLE);
@@ -1024,7 +1013,6 @@ void exynos_dp_enable_scrambling(unsigned int enable)
 int exynos_dp_init_video(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Clear VID_CLK_CHG[1] and VID_FORMAT_CHG[3] and VSYNC_DET[7] */
 	reg = VSYNC_DET | VID_FORMAT_CHG | VID_CLK_CHG;
@@ -1040,7 +1028,6 @@ int exynos_dp_init_video(void)
 void exynos_dp_config_video_slave_mode(struct edp_video_info *video_info)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Video Slave mode setting */
 	reg = readl(&dp_regs->func_en1);
@@ -1074,7 +1061,6 @@ void exynos_dp_config_video_slave_mode(struct edp_video_info *video_info)
 void exynos_dp_set_video_color_format(struct edp_video_info *video_info)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Configure the input color depth, color space, dynamic range */
 	reg = (video_info->dynamic_range << IN_D_RANGE_SHIFT) |
@@ -1097,7 +1083,6 @@ int exynos_dp_config_video_bist(struct edp_device_info *edp_info)
 	unsigned int reg;
 	unsigned int bist_type = 0;
 	struct edp_video_info video_info = edp_info->video_info;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* For master mode, you don't need to set the video format */
 	if (video_info.master_mode == 0) {
@@ -1186,7 +1171,6 @@ int exynos_dp_config_video_bist(struct edp_device_info *edp_info)
 unsigned int exynos_dp_is_slave_video_stream_clock_on(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Update Video stream clk detect status */
 	reg = readl(&dp_regs->sys_ctl1);
@@ -1206,7 +1190,6 @@ void exynos_dp_set_video_cr_mn(unsigned int type, unsigned int m_value,
 		unsigned int n_value)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	if (type == REGISTER_M) {
 		reg = readl(&dp_regs->sys_ctl4);
@@ -1235,7 +1218,6 @@ void exynos_dp_set_video_cr_mn(unsigned int type, unsigned int m_value,
 void exynos_dp_set_video_timing_mode(unsigned int type)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->video_ctl10);
 	reg &= ~FORMAT_SEL;
@@ -1249,7 +1231,6 @@ void exynos_dp_set_video_timing_mode(unsigned int type)
 void exynos_dp_enable_video_master(unsigned int enable)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	reg = readl(&dp_regs->soc_general_ctl);
 	if (enable) {
@@ -1266,7 +1247,6 @@ void exynos_dp_enable_video_master(unsigned int enable)
 void exynos_dp_start_video(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Enable Video input and disable Mute */
 	reg = readl(&dp_regs->video_ctl1);
@@ -1277,7 +1257,6 @@ void exynos_dp_start_video(void)
 unsigned int exynos_dp_is_video_stream_on(void)
 {
 	unsigned int reg;
-	struct exynos_dp *dp_regs = (struct exynos_dp *)samsung_get_base_dp();
 
 	/* Update STRM_VALID */
 	reg = readl(&dp_regs->sys_ctl3);
diff --git a/drivers/video/exynos_dp_lowlevel.h b/drivers/video/exynos_dp_lowlevel.h
index a041a7ab50..2c0ae12db0 100644
--- a/drivers/video/exynos_dp_lowlevel.h
+++ b/drivers/video/exynos_dp_lowlevel.h
@@ -76,5 +76,6 @@ void exynos_dp_set_video_timing_mode(unsigned int type);
 void exynos_dp_enable_video_master(unsigned int enable);
 void exynos_dp_start_video(void);
 unsigned int exynos_dp_is_video_stream_on(void);
+void exynos_dp_set_base_addr(void);
 
 #endif /* _EXYNOS_DP_LOWLEVEL_H */
diff --git a/drivers/video/exynos_fb.c b/drivers/video/exynos_fb.c
index c0f1830dc9..ed0823bf97 100644
--- a/drivers/video/exynos_fb.c
+++ b/drivers/video/exynos_fb.c
@@ -23,6 +23,8 @@
 #include <config.h>
 #include <common.h>
 #include <lcd.h>
+#include <fdtdec.h>
+#include <libfdt.h>
 #include <asm/io.h>
 #include <asm/arch/cpu.h>
 #include <asm/arch/clock.h>
@@ -30,6 +32,7 @@
 #include <asm/arch/mipi_dsim.h>
 #include <asm/arch/dp_info.h>
 #include <asm/arch/system.h>
+#include <asm-generic/errno.h>
 
 #include "exynos_fb.h"
 
@@ -37,6 +40,20 @@ DECLARE_GLOBAL_DATA_PTR;
 
 static unsigned int panel_width, panel_height;
 
+/*
+ * board_init_f(arch/arm/lib/board.c) calls lcd_setmem() which needs
+ * panel_info.vl_col, panel_info.vl_row and panel_info.vl_bpix to reserve
+ * FB memory at a very early stage, i.e even before exynos_fimd_parse_dt()
+ * is called. So, we are forced to statically assign it.
+ */
+#ifdef CONFIG_OF_CONTROL
+vidinfo_t panel_info  = {
+	.vl_col = LCD_XRES,
+	.vl_row = LCD_YRES,
+	.vl_bpix = LCD_COLOR16,
+};
+#endif
+
 static void exynos_lcd_init_mem(void *lcdbase, vidinfo_t *vid)
 {
 	unsigned long palette_size;
@@ -83,47 +100,226 @@ static void draw_logo(void)
 }
 #endif
 
+void __exynos_cfg_lcd_gpio(void)
+{
+}
+void exynos_cfg_lcd_gpio(void)
+	__attribute__((weak, alias("__exynos_cfg_lcd_gpio")));
+
+void __exynos_backlight_on(unsigned int onoff)
+{
+}
+void exynos_backlight_on(unsigned int onoff)
+	__attribute__((weak, alias("__exynos_cfg_lcd_gpio")));
+
+void __exynos_reset_lcd(void)
+{
+}
+void exynos_reset_lcd(void)
+	__attribute__((weak, alias("__exynos_reset_lcd")));
+
+void __exynos_lcd_power_on(void)
+{
+}
+void exynos_lcd_power_on(void)
+	__attribute__((weak, alias("__exynos_lcd_power_on")));
+
+void __exynos_cfg_ldo(void)
+{
+}
+void exynos_cfg_ldo(void)
+	__attribute__((weak, alias("__exynos_cfg_ldo")));
+
+void __exynos_enable_ldo(unsigned int onoff)
+{
+}
+void exynos_enable_ldo(unsigned int onoff)
+	__attribute__((weak, alias("__exynos_enable_ldo")));
+
+void __exynos_backlight_reset(void)
+{
+}
+void exynos_backlight_reset(void)
+	__attribute__((weak, alias("__exynos_backlight_reset")));
+
 static void lcd_panel_on(vidinfo_t *vid)
 {
 	udelay(vid->init_delay);
 
-	if (vid->backlight_reset)
-		vid->backlight_reset();
+	exynos_backlight_reset();
 
-	if (vid->cfg_gpio)
-		vid->cfg_gpio();
+	exynos_cfg_lcd_gpio();
 
-	if (vid->lcd_power_on)
-		vid->lcd_power_on();
+	exynos_lcd_power_on();
 
 	udelay(vid->power_on_delay);
 
 	if (vid->dp_enabled)
 		exynos_init_dp();
 
-	if (vid->reset_lcd) {
-		vid->reset_lcd();
-		udelay(vid->reset_delay);
-	}
+	exynos_reset_lcd();
+
+	udelay(vid->reset_delay);
 
-	if (vid->backlight_on)
-		vid->backlight_on(1);
+	exynos_backlight_on(1);
 
-	if (vid->cfg_ldo)
-		vid->cfg_ldo();
+	exynos_cfg_ldo();
 
-	if (vid->enable_ldo)
-		vid->enable_ldo(1);
+	exynos_enable_ldo(1);
 
 	if (vid->mipi_enabled)
 		exynos_mipi_dsi_init();
 }
 
+#ifdef CONFIG_OF_CONTROL
+int exynos_fimd_parse_dt(const void *blob)
+{
+	unsigned int node;
+	node = fdtdec_next_compatible(blob, 0, COMPAT_SAMSUNG_EXYNOS_FIMD);
+	if (node <= 0) {
+		debug("exynos_fb: Can't get device node for fimd\n");
+		return -ENODEV;
+	}
+
+	panel_info.vl_col = fdtdec_get_int(blob, node, "samsung,vl-col", 0);
+	if (panel_info.vl_col == 0) {
+		debug("Can't get XRES\n");
+		return -ENXIO;
+	}
+
+	panel_info.vl_row = fdtdec_get_int(blob, node, "samsung,vl-row", 0);
+	if (panel_info.vl_row == 0) {
+		debug("Can't get YRES\n");
+		return -ENXIO;
+	}
+
+	panel_info.vl_width = fdtdec_get_int(blob, node,
+						"samsung,vl-width", 0);
+
+	panel_info.vl_height = fdtdec_get_int(blob, node,
+						"samsung,vl-height", 0);
+
+	panel_info.vl_freq = fdtdec_get_int(blob, node, "samsung,vl-freq", 0);
+	if (panel_info.vl_freq == 0) {
+		debug("Can't get refresh rate\n");
+		return -ENXIO;
+	}
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-clkp"))
+		panel_info.vl_clkp = CONFIG_SYS_LOW;
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-oep"))
+		panel_info.vl_oep = CONFIG_SYS_LOW;
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-hsp"))
+		panel_info.vl_hsp = CONFIG_SYS_LOW;
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-vsp"))
+		panel_info.vl_vsp = CONFIG_SYS_LOW;
+
+	if (fdtdec_get_bool(blob, node, "samsung,vl-dp"))
+		panel_info.vl_dp = CONFIG_SYS_LOW;
+
+	panel_info.vl_bpix = fdtdec_get_int(blob, node, "samsung,vl-bpix", 0);
+	if (panel_info.vl_bpix == 0) {
+		debug("Can't get bits per pixel\n");
+		return -ENXIO;
+	}
+
+	panel_info.vl_hspw = fdtdec_get_int(blob, node, "samsung,vl-hspw", 0);
+	if (panel_info.vl_hspw == 0) {
+		debug("Can't get hsync width\n");
+		return -ENXIO;
+	}
+
+	panel_info.vl_hfpd = fdtdec_get_int(blob, node, "samsung,vl-hfpd", 0);
+	if (panel_info.vl_hfpd == 0) {
+		debug("Can't get right margin\n");
+		return -ENXIO;
+	}
+
+	panel_info.vl_hbpd = (u_char)fdtdec_get_int(blob, node,
+							"samsung,vl-hbpd", 0);
+	if (panel_info.vl_hbpd == 0) {
+		debug("Can't get left margin\n");
+		return -ENXIO;
+	}
+
+	panel_info.vl_vspw = (u_char)fdtdec_get_int(blob, node,
+							"samsung,vl-vspw", 0);
+	if (panel_info.vl_vspw == 0) {
+		debug("Can't get vsync width\n");
+		return -ENXIO;
+	}
+
+	panel_info.vl_vfpd = fdtdec_get_int(blob, node,
+							"samsung,vl-vfpd", 0);
+	if (panel_info.vl_vfpd == 0) {
+		debug("Can't get lower margin\n");
+		return -ENXIO;
+	}
+
+	panel_info.vl_vbpd = fdtdec_get_int(blob, node, "samsung,vl-vbpd", 0);
+	if (panel_info.vl_vbpd == 0) {
+		debug("Can't get upper margin\n");
+		return -ENXIO;
+	}
+
+	panel_info.vl_cmd_allow_len = fdtdec_get_int(blob, node,
+						"samsung,vl-cmd-allow-len", 0);
+
+	panel_info.win_id = fdtdec_get_int(blob, node, "samsung,winid", 0);
+	panel_info.init_delay = fdtdec_get_int(blob, node,
+						"samsung,init-delay", 0);
+	panel_info.power_on_delay = fdtdec_get_int(blob, node,
+						"samsung,power-on-delay", 0);
+	panel_info.reset_delay = fdtdec_get_int(blob, node,
+						"samsung,reset-delay", 0);
+	panel_info.interface_mode = fdtdec_get_int(blob, node,
+						"samsung,interface-mode", 0);
+	panel_info.mipi_enabled = fdtdec_get_int(blob, node,
+						"samsung,mipi-enabled", 0);
+	panel_info.dp_enabled = fdtdec_get_int(blob, node,
+						"samsung,dp-enabled", 0);
+	panel_info.cs_setup = fdtdec_get_int(blob, node,
+						"samsung,cs-setup", 0);
+	panel_info.wr_setup = fdtdec_get_int(blob, node,
+						"samsung,wr-setup", 0);
+	panel_info.wr_act = fdtdec_get_int(blob, node, "samsung,wr-act", 0);
+	panel_info.wr_hold = fdtdec_get_int(blob, node, "samsung,wr-hold", 0);
+
+	panel_info.logo_on = fdtdec_get_int(blob, node, "samsung,logo-on", 0);
+	if (panel_info.logo_on) {
+		panel_info.logo_width = fdtdec_get_int(blob, node,
+						"samsung,logo-width", 0);
+		panel_info.logo_height = fdtdec_get_int(blob, node,
+						"samsung,logo-height", 0);
+		panel_info.logo_addr = fdtdec_get_int(blob, node,
+						"samsung,logo-addr", 0);
+	}
+
+	panel_info.rgb_mode = fdtdec_get_int(blob, node,
+						"samsung,rgb-mode", 0);
+	panel_info.pclk_name = fdtdec_get_int(blob, node,
+						"samsung,pclk-name", 0);
+	panel_info.sclk_div = fdtdec_get_int(blob, node,
+						"samsung,sclk-div", 0);
+	panel_info.dual_lcd_enabled = fdtdec_get_int(blob, node,
+						"samsung,dual-lcd-enabled", 0);
+
+	return 0;
+}
+#endif
+
 void lcd_ctrl_init(void *lcdbase)
 {
 	set_system_display_ctrl();
 	set_lcd_clk();
 
+#ifdef CONFIG_OF_CONTROL
+	if (exynos_fimd_parse_dt(gd->fdt_blob))
+		debug("Can't get proper panel info\n");
+#endif
 	/* initialize parameters which is specific to panel. */
 	init_panel_info(&panel_info);
 
diff --git a/drivers/video/exynos_fimd.c b/drivers/video/exynos_fimd.c
index 2efe6a61c2..33599499ee 100644
--- a/drivers/video/exynos_fimd.c
+++ b/drivers/video/exynos_fimd.c
@@ -25,13 +25,18 @@
 #include <asm/io.h>
 #include <lcd.h>
 #include <div64.h>
+#include <fdtdec.h>
+#include <libfdt.h>
 #include <asm/arch/clk.h>
 #include <asm/arch/clock.h>
 #include <asm/arch/cpu.h>
 #include "exynos_fb.h"
 
+DECLARE_GLOBAL_DATA_PTR;
+
 static unsigned long *lcd_base_addr;
 static vidinfo_t *pvid;
+static struct exynos_fb *fimd_ctrl;
 
 void exynos_fimd_lcd_init_mem(u_long screen_base, u_long fb_size,
 		u_long palette_size)
@@ -41,8 +46,6 @@ void exynos_fimd_lcd_init_mem(u_long screen_base, u_long fb_size,
 
 static void exynos_fimd_set_dualrgb(unsigned int enabled)
 {
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 	unsigned int cfg = 0;
 
 	if (enabled) {
@@ -59,9 +62,6 @@ static void exynos_fimd_set_dualrgb(unsigned int enabled)
 
 static void exynos_fimd_set_dp_clkcon(unsigned int enabled)
 {
-
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 	unsigned int cfg = 0;
 
 	if (enabled)
@@ -73,8 +73,6 @@ static void exynos_fimd_set_dp_clkcon(unsigned int enabled)
 static void exynos_fimd_set_par(unsigned int win_id)
 {
 	unsigned int cfg = 0;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 
 	/* set window control */
 	cfg = readl((unsigned int)&fimd_ctrl->wincon0 +
@@ -126,8 +124,6 @@ static void exynos_fimd_set_par(unsigned int win_id)
 static void exynos_fimd_set_buffer_address(unsigned int win_id)
 {
 	unsigned long start_addr, end_addr;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 
 	start_addr = (unsigned long)lcd_base_addr;
 	end_addr = start_addr + ((pvid->vl_col * (NBITS(pvid->vl_bpix) / 8)) *
@@ -144,8 +140,6 @@ static void exynos_fimd_set_clock(vidinfo_t *pvid)
 	unsigned int cfg = 0, div = 0, remainder, remainder_div;
 	unsigned long pixel_clock;
 	unsigned long long src_clock;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 
 	if (pvid->dual_lcd_enabled) {
 		pixel_clock = pvid->vl_freq *
@@ -197,8 +191,6 @@ static void exynos_fimd_set_clock(vidinfo_t *pvid)
 void exynos_set_trigger(void)
 {
 	unsigned int cfg = 0;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 
 	cfg = readl(&fimd_ctrl->trigcon);
 
@@ -211,8 +203,6 @@ int exynos_is_i80_frame_done(void)
 {
 	unsigned int cfg = 0;
 	int status;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 
 	cfg = readl(&fimd_ctrl->trigcon);
 
@@ -226,8 +216,6 @@ int exynos_is_i80_frame_done(void)
 static void exynos_fimd_lcd_on(void)
 {
 	unsigned int cfg = 0;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 
 	/* display on */
 	cfg = readl(&fimd_ctrl->vidcon0);
@@ -238,8 +226,6 @@ static void exynos_fimd_lcd_on(void)
 static void exynos_fimd_window_on(unsigned int win_id)
 {
 	unsigned int cfg = 0;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 
 	/* enable window */
 	cfg = readl((unsigned int)&fimd_ctrl->wincon0 +
@@ -256,8 +242,6 @@ static void exynos_fimd_window_on(unsigned int win_id)
 void exynos_fimd_lcd_off(void)
 {
 	unsigned int cfg = 0;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 
 	cfg = readl(&fimd_ctrl->vidcon0);
 	cfg &= (EXYNOS_VIDCON0_ENVID_DISABLE | EXYNOS_VIDCON0_ENVID_F_DISABLE);
@@ -267,8 +251,6 @@ void exynos_fimd_lcd_off(void)
 void exynos_fimd_window_off(unsigned int win_id)
 {
 	unsigned int cfg = 0;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
 
 	cfg = readl((unsigned int)&fimd_ctrl->wincon0 +
 			EXYNOS_WINCON(win_id));
@@ -286,8 +268,20 @@ void exynos_fimd_lcd_init(vidinfo_t *vid)
 {
 	unsigned int cfg = 0, rgb_mode;
 	unsigned int offset;
-	struct exynos_fb *fimd_ctrl =
-		(struct exynos_fb *)samsung_get_base_fimd();
+#ifdef CONFIG_OF_CONTROL
+	unsigned int node;
+
+	node = fdtdec_next_compatible(gd->fdt_blob,
+					0, COMPAT_SAMSUNG_EXYNOS_FIMD);
+	if (node <= 0)
+		debug("exynos_fb: Can't get device node for fimd\n");
+
+	fimd_ctrl = (struct exynos_fb *)fdtdec_get_addr(gd->fdt_blob,
+								node, "reg");
+	if (fimd_ctrl == NULL)
+		debug("Can't get the FIMD base address\n");
+#endif
+	fimd_ctrl = (struct exynos_fb *)samsung_get_base_fimd();
 
 	offset = exynos_fimd_get_base_offset();