i.MX35 3stack: several enhancements

- Add support for booting from NAND
- Add support for internal Boot mode
- Add I2C PMIC support
- Enable FEC Ethernet controller

Signed-off-by: Marc Kleine-Budde <mkl@pengutronix.de>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

5 files changed, 547 insertions, 401 deletions

diff --git a/board/freescale-mx35-3-stack/3stack.c b/board/freescale-mx35-3-stack/3stack.c
index ade5d12f98..63f7bbacc8 100644
--- a/board/freescale-mx35-3-stack/3stack.c
+++ b/board/freescale-mx35-3-stack/3stack.c
@@ -1,5 +1,6 @@
 /*
- * Copyright (C) 2007 Sascha Hauer, Pengutronix 
+ * Copyright (C) 2007 Sascha Hauer, Pengutronix
+ *               2009 Marc Kleine-Budde, Pengutronix
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -16,86 +17,148 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
  * MA 02111-1307 USA
  *
+ * Derived from:
+ *
+ * * mx35_3stack.c - board file for u-boot-v1
+ *   Copyright (C) 2007, Guennadi Liakhovetski <lg@denx.de>
+ *   (C) Copyright 2008-2009 Freescale Semiconductor, Inc.
+ *
  */
 
 #include <common.h>
-#include <net.h>
 #include <cfi_flash.h>
-#include <init.h>
 #include <environment.h>
-#include <mach/imx-regs.h>
+#include <errno.h>
+#include <fcntl.h>
 #include <fec.h>
-#include <mach/gpio.h>
-#include <asm/armlinux.h>
-#include <asm/mach-types.h>
-#include <mach/pmic.h>
-#include <partition.h>
 #include <fs.h>
-#include <fcntl.h>
+#include <init.h>
 #include <nand.h>
-#include <spi/spi.h>
+#include <net.h>
+#include <partition.h>
+
+#include <asm/armlinux.h>
 #include <asm/io.h>
+#include <asm/mach-types.h>
+
+#include <mach/gpio.h>
 #include <mach/imx-nand.h>
+#include <mach/imx-regs.h>
 #include <mach/iomux-mx35.h>
+#include <mach/iomux-v3.h>
+#include <mach/pmic.h>
+
+#include <i2c/i2c.h>
+#include <i2c/mc13892.h>
+#include <i2c/mc9sdz60.h>
 
 static struct device_d cfi_dev = {
-	.name     = "cfi_flash",
-	.map_base = 0xa0000000,
-	.size     = 64 * 1024 * 1024,
+	.name		= "cfi_flash",
+	.map_base	= IMX_CS0_BASE,
+	.size		= 64 * 1024 * 1024,
 };
 
-static struct memory_platform_data ram_pdata = {
+static struct fec_platform_data fec_info = {
+	.xcv_type = MII100,
+	.phy_addr = 0x1F,
+};
+
+static struct device_d fec_dev = {
+	.name		= "fec_imx",
+	.map_base	= IMX_FEC_BASE,
+	.platform_data	= &fec_info,
+};
+
+static struct memory_platform_data sdram_pdata = {
 	.name = "ram0",
 	.flags = DEVFS_RDWR,
 };
 
 static struct device_d sdram_dev = {
-	.name     = "mem",
-	.map_base = 0x80000000,
-	.size     = 128 * 1024 * 1024,
-	.platform_data = &ram_pdata,
+	.name		= "mem",
+	.map_base	= IMX_SDRAM_CS0,
+	.size		= 128 * 1024 * 1024,
+	.platform_data	= &sdram_pdata,
 };
 
-static struct fec_platform_data fec_info = {
-	.xcv_type = MII100,
+struct imx_nand_platform_data nand_info = {
+	.hw_ecc		= 1,
+	.flash_bbt	= 1,
 };
 
-static struct device_d fec_dev = {
-	.name     = "fec_imx",
-	.map_base = 0x50038000,
-	.platform_data	= &fec_info,
+static struct device_d nand_dev = {
+	.name		= "imx_nand",
+	.map_base	= IMX_NFC_BASE,
+	.platform_data	= &nand_info,
 };
 
-/*
- * SMSC 9217 network controller
- */
 static struct device_d smc911x_dev = {
-	.name     = "smc911x",
-	.map_base = IMX_CS5_BASE,
-	.size     = IMX_CS5_RANGE,	/* area size */
+	.name		= "smc911x",
+	.map_base	= IMX_CS5_BASE,
+	.size		= IMX_CS5_RANGE,
+};
+
+static struct i2c_board_info i2c_devices[] = {
+	{
+		I2C_BOARD_INFO("mc13892", 0x08),
+	}, {
+		I2C_BOARD_INFO("mc9sdz60", 0x69),
+	},
+};
+
+static struct device_d i2c_dev = {
+	.name		= "i2c-imx",
+	.map_base	= IMX_I2C1_BASE,
 };
 
 static int f3s_devices_init(void)
 {
-	register_device(&cfi_dev);
-	register_device(&sdram_dev);
-	register_device(&smc911x_dev);
-	/* FEC is currently broken. It seems to work
-	 * shortly but after a few moments the board
-	 * goes to nirvana
-	 */
-//	register_device(&fec_dev);
+	uint32_t reg;
+
+	/* CS0: Nor Flash */
+	writel(0x0000cf03, CSCR_U(0));
+	writel(0x10000d03, CSCR_L(0));
+	writel(0x00720900, CSCR_A(0));
+
+	reg = readl(IMX_CCM_BASE + CCM_RCSR);
+	/* some fuses provide us vital information about connected hardware */
+	if (reg & 0x20000000)
+		nand_info.width = 2;	/* bit */
+	else
+		nand_info.width = 1;	/* 8 bit */
 
 	/*
-	 * Create partitions that should be
-	 * not touched by any regular user
+	 * This platform supports NOR and NAND
 	 */
-	devfs_add_partition("nor0", 0x00000, 0x40000, PARTITION_FIXED, "self0");	/* ourself */
-	devfs_add_partition("nor0", 0x40000, 0x20000, PARTITION_FIXED, "env0");	/* environment */
+	register_device(&nand_dev);
+	register_device(&cfi_dev);
+
+	switch ( (reg >> 25) & 0x3) {
+	case 0x01:		/* NAND is the source */
+		devfs_add_partition("nand0", 0x00000, 0x40000, PARTITION_FIXED, "self_raw");
+		dev_add_bb_dev("self_raw", "self0");
+		devfs_add_partition("nand0", 0x40000, 0x20000, PARTITION_FIXED, "env_raw");
+		dev_add_bb_dev("env_raw", "env0");
+		break;
+
+	case 0x00:		/* NOR is the source */
+		devfs_add_partition("nor0", 0x00000, 0x40000, PARTITION_FIXED, "self0");
+		devfs_add_partition("nor0", 0x40000, 0x20000, PARTITION_FIXED, "env0");
+		protect_file("/dev/env0", 1);
+		break;
+	}
+
+	i2c_register_board_info(0, i2c_devices, ARRAY_SIZE(i2c_devices));
+	register_device(&i2c_dev);
+
+	register_device(&fec_dev);
+	register_device(&smc911x_dev);
+
+	register_device(&sdram_dev);
 
 	armlinux_add_dram(&sdram_dev);
 	armlinux_set_bootparams((void *)0x80000100);
-	armlinux_set_architecture(MACH_TYPE_PCM037); /* FIXME */
+	armlinux_set_architecture(MACH_TYPE_MX35_3DS);
 
 	return 0;
 }
@@ -103,9 +166,9 @@ static int f3s_devices_init(void)
 device_initcall(f3s_devices_init);
 
 static struct device_d f3s_serial_device = {
-	.name     = "imx_serial",
-	.map_base = IMX_UART1_BASE,
-	.size     = 4096,
+	.name		= "imx_serial",
+	.map_base	= IMX_UART1_BASE,
+	.size		= 4096,
 };
 
 static struct pad_desc f3s_pads[] = {
@@ -129,10 +192,16 @@ static struct pad_desc f3s_pads[] = {
 	MX35_PAD_FEC_TDATA2__FEC_TDATA_2,
 	MX35_PAD_FEC_RDATA3__FEC_RDATA_3,
 	MX35_PAD_FEC_TDATA3__FEC_TDATA_3,
+
 	MX35_PAD_RXD1__UART1_RXD_MUX,
 	MX35_PAD_TXD1__UART1_TXD_MUX,
 	MX35_PAD_RTS1__UART1_RTS,
 	MX35_PAD_CTS1__UART1_CTS,
+
+	MX35_PAD_I2C1_CLK__I2C1_SCL,
+	MX35_PAD_I2C1_DAT__I2C1_SDA,
+
+	MX35_PAD_WDOG_RST__GPIO1_6,
 };
 
 static int f3s_console_init(void)
@@ -145,18 +214,19 @@ static int f3s_console_init(void)
 
 console_initcall(f3s_console_init);
 
-static int f3s_core_setup(void)
+static int f3s_core_init(void)
 {
-	u32 tmp;
+	u32 reg;
 
 	writel(0x0000D843, CSCR_U(5)); /* CS5: smc9117 */
 	writel(0x22252521, CSCR_L(5));
 	writel(0x22220A00, CSCR_A(5));
 
-	/* FIXME: The rest is currently done in Assembler. Remove assembler
-	 *        config once the board is running stable
-	 */
-	return 0;
+	/* enable clock for I2C1 and FEC */
+	reg = readl(IMX_CCM_BASE + CCM_CGR1);
+	reg |= 0x3 << CCM_CGR1_FEC_SHIFT;
+	reg |= 0x3 << CCM_CGR1_I2C1_SHIFT;
+	reg = writel(reg, IMX_CCM_BASE + CCM_CGR1);
 
 	/* AIPS setup - Only setup MPROTx registers. The PACR default values are good.*/
 	/*
@@ -177,17 +247,17 @@ static int f3s_core_setup(void)
 	writel(0x0, IMX_AIPS1_BASE + 0x44);
 	writel(0x0, IMX_AIPS1_BASE + 0x48);
 	writel(0x0, IMX_AIPS1_BASE + 0x4C);
-	tmp = readl(IMX_AIPS1_BASE + 0x50);
-	tmp &= 0x00FFFFFF;
-	writel(tmp, IMX_AIPS1_BASE + 0x50);
+	reg = readl(IMX_AIPS1_BASE + 0x50);
+	reg &= 0x00FFFFFF;
+	writel(reg, IMX_AIPS1_BASE + 0x50);
 
 	writel(0x0, IMX_AIPS2_BASE + 0x40);
 	writel(0x0, IMX_AIPS2_BASE + 0x44);
 	writel(0x0, IMX_AIPS2_BASE + 0x48);
 	writel(0x0, IMX_AIPS2_BASE + 0x4C);
-	tmp = readl(IMX_AIPS2_BASE + 0x50);
-	tmp &= 0x00FFFFFF;
-	writel(tmp, IMX_AIPS2_BASE + 0x50);
+	reg = readl(IMX_AIPS2_BASE + 0x50);
+	reg &= 0x00FFFFFF;
+	writel(reg, IMX_AIPS2_BASE + 0x50);
 
 	/* MAX (Multi-Layer AHB Crossbar Switch) setup */
 
@@ -217,5 +287,90 @@ static int f3s_core_setup(void)
 	return 0;
 }
 
-core_initcall(f3s_core_setup);
+core_initcall(f3s_core_init);
+
+static int f3s_get_rev(struct i2c_client *client)
+{
+	u8 reg[3];
+	int rev;
+
+	i2c_read_reg(client, 0x7, reg, sizeof(reg));
+
+	rev = reg[0] << 16 | reg [1] << 8 | reg[2];
+	dev_info(&client->dev, "revision: 0x%x\n", rev);
+
+	/* just return '0' or '1' */
+	return !!((rev >> 6) & 0x7);
+}
+
+static void f3s_pmic_init_v2(struct i2c_client *client)
+{
+	u8 reg[3];
+
+	i2c_read_reg(client, 0x1e, reg, sizeof(reg));
+	reg[2] |= 0x03;
+	i2c_write_reg(client, 0x1e, reg, sizeof(reg));
+
+	i2c_read_reg(client, 0x20, reg, sizeof(reg));
+	reg[2] |= 0x01;
+	i2c_write_reg(client, 0x20, reg, sizeof(reg));
+}
+
+static void f3s_pmic_init_all(struct i2c_client *client)
+{
+	u8 reg[1];
+
+	i2c_read_reg(client, 0x20, reg, sizeof(reg));
+	reg[0] |= 0x04;
+	i2c_write_reg(client, 0x20, reg, sizeof(reg));
+
+	mdelay(200);
+
+	i2c_read_reg(client, 0x1a, reg, sizeof(reg));
+	reg[0] &= 0x7f;
+	i2c_write_reg(client, 0x1a, reg, sizeof(reg));
+
+	mdelay(200);
+
+	reg[0] |= 0x80;
+	i2c_write_reg(client, 0x1a, reg, sizeof(reg));
+}
+
+static int f3s_pmic_init(void)
+{
+	struct i2c_client *client;
+	int rev;
+
+	client = mc13892_get_client();
+	if (!client)
+		return -ENODEV;
+
+	rev = f3s_get_rev(client);
+	if (rev) {
+		printf("i.MX35 CPU board version 2.0\n");
+		f3s_pmic_init_v2(client);
+	} else {
+		printf("i.MX35 CPU board version 1.0\n");
+	}
+
+	client = mc9sdz60_get_client();
+	if (!client)
+		return -ENODEV;
+	f3s_pmic_init_all(client);
+
+	return 0;
+}
+
+late_initcall(f3s_pmic_init);
+
+#ifdef CONFIG_NAND_IMX_BOOT
+void __bare_init nand_boot(void)
+{
+	/*
+	 * The driver is able to detect NAND's pagesize by CPU internal
+	 * fuses or external pull ups. But not the blocksize...
+	 */
+	imx_nand_load_image((void *)TEXT_BASE, 256 * 1024);
+}
+#endif
 
diff --git a/board/freescale-mx35-3-stack/Makefile b/board/freescale-mx35-3-stack/Makefile
index 1ad0ce984c..a8ea4a3d66 100644
--- a/board/freescale-mx35-3-stack/Makefile
+++ b/board/freescale-mx35-3-stack/Makefile
@@ -1,3 +1,4 @@
 
-obj-y += lowlevel_init.o
-obj-y += 3stack.o
+obj-y					+= lowlevel_init.o
+obj-y					+= 3stack.o
+obj-$(CONFIG_ARCH_IMX_INTERNAL_BOOT)	+= flash_header.o
diff --git a/board/freescale-mx35-3-stack/board-mx35_3stack.h b/board/freescale-mx35-3-stack/board-mx35_3stack.h
new file mode 100644
index 0000000000..c18066ad9c
--- /dev/null
+++ b/board/freescale-mx35-3-stack/board-mx35_3stack.h
@@ -0,0 +1,107 @@
+/*
+ *
+ * (c) 2007 Pengutronix, Sascha Hauer <s.hauer@pengutronix.de>
+ *
+ * (C) Copyright 2008 Freescale Semiconductor, Inc.
+ *
+ * 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 __BOARD_MX35_3STACK_H
+#define __BOARD_MX35_3STACK_H
+
+#define UNALIGNED_ACCESS_ENABLE
+#define LOW_INT_LATENCY_ENABLE
+#define BRANCH_PREDICTION_ENABLE
+
+#define L2CC_AUX_CTL_CONFIG	0x00030024
+
+#define AIPS_MPR_CONFIG		0x77777777
+#define AIPS_OPACR_CONFIG	0x00000000
+
+/* MPR - priority is M4 > M2 > M3 > M5 > M0 > M1 */
+#define MAX_MPR_CONFIG		0x00302154
+/* SGPCR - always park on last master */
+#define MAX_SGPCR_CONFIG	0x00000010
+/* MGPCR - restore default values */
+#define MAX_MGPCR_CONFIG	0x00000000
+
+/*
+ * M3IF Control Register (M3IFCTL)
+ * MRRP[0] = L2CC0 not on priority list (0 << 0) = 0x00000000
+ * MRRP[1] = L2CC1 not on priority list (0 << 0) = 0x00000000
+ * MRRP[2] = MBX not on priority list (0 << 0)   = 0x00000000
+ * MRRP[3] = MAX1 not on priority list (0 << 0)  = 0x00000000
+ * MRRP[4] = SDMA not on priority list (0 << 0)  = 0x00000000
+ * MRRP[5] = MPEG4 not on priority list (0 << 0) = 0x00000000
+ * MRRP[6] = IPU1 on priority list (1 << 6)      = 0x00000040
+ * MRRP[7] = IPU2 not on priority list (0 << 0)  = 0x00000000
+ *                                               ------------
+ *                                                 0x00000040
+ */
+#define M3IF_CONFIG	0x00000040
+
+#define DBG_BASE_ADDR		WEIM_CTRL_CS5
+#define DBG_CSCR_U_CONFIG	0x0000D843
+#define DBG_CSCR_L_CONFIG	0x22252521
+#define DBG_CSCR_A_CONFIG	0x22220A00
+
+#define CCM_CCMR_CONFIG		0x003F4208
+#define CCM_PDR0_CONFIG		0x00821000
+
+#define PLL_BRM_OFFSET	31
+#define PLL_PD_OFFSET	26
+#define PLL_MFD_OFFSET	16
+#define PLL_MFI_OFFSET	10
+
+#define _PLL_BRM(x)	((x) << PLL_BRM_OFFSET)
+#define _PLL_PD(x)	(((x) - 1) << PLL_PD_OFFSET)
+#define _PLL_MFD(x)	(((x) - 1) << PLL_MFD_OFFSET)
+#define _PLL_MFI(x)	((x) << PLL_MFI_OFFSET)
+#define _PLL_MFN(x)	(x)
+#define _PLL_SETTING(brm, pd, mfd, mfi, mfn) \
+	(_PLL_BRM(brm) | _PLL_PD(pd) | _PLL_MFD(mfd) | _PLL_MFI(mfi) |\
+	 _PLL_MFN(mfn))
+
+#define CCM_MPLL_532_HZ	_PLL_SETTING(1, 1, 12, 11, 1)
+#define CCM_MPLL_399_HZ _PLL_SETTING(0, 1, 16, 8, 5)
+#define CCM_PPLL_300_HZ _PLL_SETTING(0, 1, 4, 6, 1)
+
+/*MEMORY SETING*/
+#define ESDCTL_0x92220000	0x92220000
+#define ESDCTL_0xA2220000	0xA2220000
+#define ESDCTL_0xB2220000	0xB2220000
+#define ESDCTL_0x82228080 	0x82228080
+
+#define ESDCTL_PRECHARGE	0x00000400
+
+#define ESDCTL_MDDR_CONFIG 	0x007FFC3F
+#define ESDCTL_MDDR_MR		0x00000033
+#define ESDCTL_MDDR_EMR		0x02000000
+
+#define ESDCTL_DDR2_CONFIG 	0x007FFC3F
+#define ESDCTL_DDR2_EMR2	0x04000000
+#define ESDCTL_DDR2_EMR3	0x06000000
+#define ESDCTL_DDR2_EN_DLL	0x02000400
+#define ESDCTL_DDR2_RESET_DLL	0x00000333
+#define ESDCTL_DDR2_MR		0x00000233
+#define ESDCTL_DDR2_OCD_DEFAULT 0x02000780
+
+#define ESDCTL_DELAY_LINE5	0x00F49F00
+#endif				/* __BOARD_MX35_3STACK_H */
diff --git a/board/freescale-mx35-3-stack/flash_header.c b/board/freescale-mx35-3-stack/flash_header.c
new file mode 100644
index 0000000000..a2496a52d0
--- /dev/null
+++ b/board/freescale-mx35-3-stack/flash_header.c
@@ -0,0 +1,49 @@
+#include <common.h>
+#include <mach/imx-flash-header.h>
+
+extern unsigned long _stext;
+
+void __naked __flash_header_start go(void)
+{
+	__asm__ __volatile__("b _start\n");
+}
+
+struct imx_dcd_entry __dcd_entry_0x400 dcd_entry[] = {
+	{ .ptr_type = 4, .addr = 0xb8002050, .val = 0x0000d843, },
+	{ .ptr_type = 4, .addr = 0xB8002054, .val = 0x22252521, },
+	{ .ptr_type = 4, .addr = 0xB8002058, .val = 0x22220a00, },
+	{ .ptr_type = 4, .addr = 0xB8001010, .val = 0x00000304, },
+	{ .ptr_type = 4, .addr = 0xB8001010, .val = 0x0000030C, },
+	{ .ptr_type = 4, .addr = 0xB8001004, .val = 0x007ffc3f, },
+	{ .ptr_type = 4, .addr = 0xB8001000, .val = 0x92220000, },
+	{ .ptr_type = 4, .addr = 0x80000400, .val = 0x12345678, },
+	{ .ptr_type = 4, .addr = 0xB8001000, .val = 0xA2220000, },
+	{ .ptr_type = 4, .addr = 0x80000000, .val = 0x87654321, },
+	{ .ptr_type = 4, .addr = 0x80000000, .val = 0x87654321, },
+	{ .ptr_type = 4, .addr = 0xB8001000, .val = 0xB2220000, },
+	{ .ptr_type = 1, .addr = 0x80000233, .val = 0xda, },
+	{ .ptr_type = 1, .addr = 0x82000780, .val = 0xda, },
+	{ .ptr_type = 1, .addr = 0x82000400, .val = 0xda, },
+	{ .ptr_type = 4, .addr = 0xB8001000, .val = 0x82226080, },
+	{ .ptr_type = 4, .addr = 0xB8001004, .val = 0x007ffc3f, },
+	{ .ptr_type = 4, .addr = 0xB800100C, .val = 0x007ffc3f, },
+	{ .ptr_type = 4, .addr = 0xB8001010, .val = 0x00000304, },
+	{ .ptr_type = 4, .addr = 0xB8001008, .val = 0x00002000, },
+};
+
+#define APP_DEST	0x80000000
+
+struct imx_flash_header __flash_header_0x400 flash_header = {
+	.app_code_jump_vector	= APP_DEST + 0x1000,
+	.app_code_barker	= APP_CODE_BARKER,
+	.app_code_csf		= 0,
+	.dcd_ptr_ptr		= APP_DEST + 0x400 + offsetof(struct imx_flash_header, dcd),
+	.super_root_key		= 0,
+	.dcd			= APP_DEST + 0x400 + offsetof(struct imx_flash_header, dcd_barker),
+	.app_dest		= APP_DEST,
+	.dcd_barker		= DCD_BARKER,
+	.dcd_block_len		= sizeof (dcd_entry),
+};
+
+unsigned long __image_len_0x400 u_boot_len = 0x40000;
+
diff --git a/board/freescale-mx35-3-stack/lowlevel_init.S b/board/freescale-mx35-3-stack/lowlevel_init.S
index f912a3a1b0..a00d2af056 100644
--- a/board/freescale-mx35-3-stack/lowlevel_init.S
+++ b/board/freescale-mx35-3-stack/lowlevel_init.S
@@ -1,296 +1,114 @@
 /*
- * For clock initialization, see chapter 3 of the "MCIMX27 Multimedia
- * Applications Processor Reference Manual, Rev. 0.2".
  *
+ * (c) 2007 Pengutronix, Sascha Hauer <s.hauer@pengutronix.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 <config.h>
 #include <mach/imx-regs.h>
+#include <mach/imx-pll.h>
+#include <mach/esdctl.h>
+#include <asm/cache-l2x0.h>
+#include "board-mx35_3stack.h"
+
+#define CSD0_BASE_ADDR             0x80000000
+#define ESDCTL_BASE_ADDR        0xB8001000
+#define CSD1_BASE_ADDR             0x90000000
 
 #define writel(val, reg) \
 	ldr		r0,	=reg;	\
 	ldr		r1,	=val;	\
-	str		r1,   [r0];
-#define IIM_BASE_ADDR           0x53FF0000
-#define SDRAM_BASE_ADDR             CSD0_BASE_ADDR
-#define CSD0_BASE_ADDR          0x80000000
-#define IIM_SREV_OFF            0x24
-#define AIPS1_CTRL_BASE_ADDR    AIPS1_BASE_ADDR
-#define AIPS1_BASE_ADDR         0x43F00000
-#define AIPS2_CTRL_BASE_ADDR    AIPS2_BASE_ADDR
-#define AIPS2_BASE_ADDR         0x53F00000
-#define MAX_BASE_ADDR           0x43F04000
-#define CLKCTL_BASE_ADDR        0x43F0C000
-#define ESDCTL_BASE             0xB8001000
-#define M3IF_BASE               0xB8003000
-#define IOMUXC_BASE_ADDR        0x43FAC000
-#define MPCTL_PARAM_399     (((1-1) << 26) + ((16-1) << 16) + (8  << 10) + (5 << 0))
-#define MPCTL_PARAM_532     ((1 << 31) + ((1-1) << 26) + ((12-1) << 16) + (11  << 10) + (1 << 0))
-#define PPCTL_PARAM_300     (((1-1) << 26) + ((4-1) << 16) + (6  << 10) + (1  << 0))
-#define SDRAM_SIZE                  0x08000000
-#define CCM_BASE_ADDR           0x53F80000
-#define IPU_CTRL_BASE_ADDR      0x53FC0000
-#define WEIM_CTRL_CS5           (WEIM_BASE_ADDR + 0x50)
-#define WEIM_BASE_ADDR          0xB8002000
-#define WEIM_CTRL_CS0           WEIM_BASE_ADDR
+	str		r1,	[r0];
+
+#define writeb(val, reg) \
+	ldr		r0,	=reg;	\
+	ldr		r1,	=val;	\
+	strb		r1,	[r0];
+
+/* Assuming 24MHz input clock */
+#define MPCTL_PARAM_399     (IMX_PLL_PD(0) | IMX_PLL_MFD(15) | IMX_PLL_MFI(8) | IMX_PLL_MFN(5))
+#define MPCTL_PARAM_532     ((1 << 31) | IMX_PLL_PD(0) | IMX_PLL_MFD(11) | IMX_PLL_MFI(11) | IMX_PLL_MFN(1))
+#define PPCTL_PARAM_300     (IMX_PLL_PD(0) | IMX_PLL_MFD(3) | IMX_PLL_MFI(6) | IMX_PLL_MFN(1))
+
+	.section ".text_bare_init","ax"
 
 ARM_PPMRR:              .word   0x40000015
 L2CACHE_PARAM:          .word   0x00030024
-IIM_SREV_REG_VAL:       .word   IIM_BASE_ADDR + IIM_SREV_OFF
-AIPS1_CTRL_BASE_ADDR_W: .word   AIPS1_CTRL_BASE_ADDR
-AIPS2_CTRL_BASE_ADDR_W: .word   AIPS2_CTRL_BASE_ADDR
-AIPS1_PARAM_W:          .word   0x77777777
-MAX_BASE_ADDR_W:        .word   MAX_BASE_ADDR
-MAX_PARAM1:             .word   0x00302154
-CLKCTL_BASE_ADDR_W:     .word   CLKCTL_BASE_ADDR
-ESDCTL_BASE_W:          .word   ESDCTL_BASE
-M3IF_BASE_W:            .word   M3IF_BASE
-RAM_PARAM1_MDDR:	.word	0x00000400
-RAM_PARAM2_MDDR:	.word	0x00000333
-RAM_PARAM3_MDDR:	.word	0x02000400
-			.word	0x02000000
-RAM_PARAM4_MDDR:	.word	0x04000000
-RAM_PARAM5_MDDR:	.word	0x06000000
-RAM_PARAM6_MDDR:	.word	0x00000233
-			.word	0x00000033
-RAM_PARAM7_MDDR:	.word	0x02000780
-ESDCTL_0x92220000:      .word   0x92220000
-ESDCTL_0xA2220000:      .word   0xA2220000
-ESDCTL_0xB2220000:      .word   0xB2220000
-ESDCTL_0x82226080:      .word   0x82226080
-ESDCTL_CONFIG:       	.word   0x007FFC3F	//DDR2
-			.word 	0x00295729	//MDDR
-ESDCTL_DELAY5:		.word   0x00F49F00
-IOMUXC_BASE_ADDR_W:     .word   IOMUXC_BASE_ADDR
 CCM_CCMR_W:             .word   0x003F4208
-//CCM_PDR0_W:             .word   0x00801000
-CCM_PDR0_W:             .word   0x00801c00
+CCM_PDR0_W:             .word   0x00001000
 MPCTL_PARAM_399_W:      .word   MPCTL_PARAM_399
 MPCTL_PARAM_532_W:      .word   MPCTL_PARAM_532
 PPCTL_PARAM_W:    	.word   PPCTL_PARAM_300
-MXC_REDBOOT_ROM_START:  .word   SDRAM_BASE_ADDR + SDRAM_SIZE - 0x100000
-CONST_0x0FFF:           .word   0x0FFF
-CCM_BASE_ADDR_W:        .word   CCM_BASE_ADDR
-IPU_CTRL_BASE_ADDR_W:   .word   IPU_CTRL_BASE_ADDR
-WEIM_CTRL_CS5_W:    .word   WEIM_CTRL_CS5
-WEIM_CTRL_CS0_W:    .word   WEIM_CTRL_CS0
-CS0_CSCRU_0x0000CC03:   .word   0x0000DCF6
-CS0_CSCRL_0xA0330D01:   .word   0x444A4541
-CS0_CSCRA_0x00220800:   .word   0x44443302
-CS5_CSCRU_0x0000D843:   .word   0x0000D843
-CS5_CSCRL_0x22252521:   .word   0x22252521
-CS5_CSCRA_0x22220A00:   .word   0x22220A00
-
-#define L2CC_BASE_ADDR          0x30000000
-#define L2_CACHE_CTL_REG                0x100
-#define L2_CACHE_AUX_CTL_REG            0x104
-#define L2_CACHE_DBG_CTL_REG      0xF40
-#define L2_CACHE_INV_WAY_REG            0x77C
-
-/* Assuming 24MHz input clock */
-/*                            PD             MFD              MFI          MFN */
-#define MPCTL_PARAM_399     (((1-1) << 26) + ((16-1) << 16) + (8  << 10) + (5 << 0))
-#define MPCTL_PARAM_532     ((1 << 31) + ((1-1) << 26) + ((12-1) << 16) + (11  << 10) + (1 << 0))
-#define MPCTL_PARAM_665     (((1-1) << 26) + ((48-1) << 16) + (13  << 10) + (41 << 0))
-#define PPCTL_PARAM_300     (((1-1) << 26) + ((4-1) << 16) + (6  << 10) + (1  << 0))
-
-#define M3IF_BASE               0xB8003000
-
-#define UNALIGNED_ACCESS_ENABLE
-#define LOW_INT_LATENCY_ENABLE
-#define BRANCH_PREDICTION_ENABLE
+CCM_BASE_ADDR_W:        .word   IMX_CCM_BASE
 
 .globl board_init_lowlevel
 board_init_lowlevel:
 	mov     r10, lr
 
-    mrc 15, 0, r1, c1, c0, 0
-    bic r1, r1, #(0x3<<21)
-    bic r1, r1, #(0x3<<11)
-    bic r1, r1, #0x5
-
-    bic r1, r1, #(1<<3)
-
-#ifndef BRANCH_PREDICTION_ENABLE
-    mrc 15, 0, r0, c1, c0, 1
-    bic r0, r0, #7
-    mcr 15, 0, r0, c1, c0, 1
-#else
-    mrc 15, 0, r0, c1, c0, 1
-    orr r0, r0, #7
-    mcr 15, 0, r0, c1, c0, 1
-    orr r1, r1, #(1<<11)
-#endif
-
-#ifdef UNALIGNED_ACCESS_ENABLE
-    orr r1, r1, #(1<<22)
-#endif
-
-#ifdef LOW_INT_LATENCY_ENABLE
-    orr r1, r1, #(1<<21)
-#endif
-    mcr 15, 0, r1, c1, c0, 0
-
-#ifdef BRANCH_PREDICTION_ENABLE
-    mov r0, #0
-    mcr 15, 0, r0, c15, c2, 4
-#endif
-
-    mov r0, #0
-    mcr 15, 0, r0, c7, c7, 0        /* invalidate I cache and D cache */
-    mcr 15, 0, r0, c8, c7, 0        /* invalidate TLBs */
-    mcr 15, 0, r0, c7, c10, 4       /* Drain the write buffer */
-
-    /* Also setup the Peripheral Port Remap register inside the core */
-    ldr r0, ARM_PPMRR        /* start from AIPS 2GB region */
-    mcr p15, 0, r0, c15, c2, 4
-
-    /*** L2 Cache setup/invalidation/disable ***/
-    /* Disable L2 cache first */
-    mov r0, #L2CC_BASE_ADDR
-    ldr r2, [r0, #L2_CACHE_CTL_REG]
-    bic r2, r2, #0x1
-    str r2, [r0, #L2_CACHE_CTL_REG]
-    /*
-     * Configure L2 Cache:
-     * - 128k size(16k way)
-     * - 8-way associativity
-     * - 0 ws TAG/VALID/DIRTY
-     * - 4 ws DATA R/W
-     */
-    ldr r1, [r0, #L2_CACHE_AUX_CTL_REG]
-    and r1, r1, #0xFE000000
-    ldr r2, L2CACHE_PARAM
-    orr r1, r1, r2
-    str r1, [r0, #L2_CACHE_AUX_CTL_REG]
-/* Workaournd for DDR issue:WT*/
-    ldr r1, [r0, #L2_CACHE_DBG_CTL_REG]
-    orr r1, r1, #2
-    str r1, [r0, #L2_CACHE_DBG_CTL_REG]
-
-    /* Invalidate L2 */
-    mov r1, #0x000000FF
-    str r1, [r0, #L2_CACHE_INV_WAY_REG]
-L2_loop:
-    /* Poll Invalidate By Way register */
-    ldr r2, [r0, #L2_CACHE_INV_WAY_REG]
-    cmp r2, #0
-    bne L2_loop
-    /*** End of L2 operations ***/
+	mrc 15, 0, r1, c1, c0, 0
 
-/*
- * End of ARM1136 init
- */
+	mrc 15, 0, r0, c1, c0, 1
+	orr r0, r0, #7
+	mcr 15, 0, r0, c1, c0, 1
+
+	orr r1, r1, #(1<<11) /* Flow prediction (Z) */
+	orr r1, r1, #(1<<22) /* unaligned accesses  */
+	orr r1, r1, #(1<<21) /* Low Int Latency     */
+
+	mcr 15, 0, r1, c1, c0, 0
+
+	mov r0, #0
+	mcr 15, 0, r0, c15, c2, 4
 
-        /*
-         * Set all MPROTx to be non-bufferable, trusted for R/W,
-         * not forced to user-mode.
-         */
-        ldr r0, AIPS1_CTRL_BASE_ADDR_W
-        ldr r1, AIPS1_PARAM_W
-        str r1, [r0, #0x00]
-        str r1, [r0, #0x04]
-        ldr r0, AIPS2_CTRL_BASE_ADDR_W
-        str r1, [r0, #0x00]
-        str r1, [r0, #0x04]
-
-        /*
-         * Clear the on and off peripheral modules Supervisor Protect bit
-         * for SDMA to access them. Did not change the AIPS control registers
-         * (offset 0x20) access type
-         */
-        ldr r0, AIPS1_CTRL_BASE_ADDR_W
-        ldr r1, =0x0
-        str r1, [r0, #0x40]
-        str r1, [r0, #0x44]
-        str r1, [r0, #0x48]
-        str r1, [r0, #0x4C]
-        ldr r1, [r0, #0x50]
-        and r1, r1, #0x00FFFFFF
-        str r1, [r0, #0x50]
-
-        ldr r0, AIPS2_CTRL_BASE_ADDR_W
-        ldr r1, =0x0
-        str r1, [r0, #0x40]
-        str r1, [r0, #0x44]
-        str r1, [r0, #0x48]
-        str r1, [r0, #0x4C]
-        ldr r1, [r0, #0x50]
-        and r1, r1, #0x00FFFFFF
-        str r1, [r0, #0x50]
-
-        ldr r0, MAX_BASE_ADDR_W
-        /* MPR - priority is M4 > M2 > M3 > M5 > M0 > M1 */
-        ldr r1, MAX_PARAM1
-        str r1, [r0, #0x000]        /* for S0 */
-        str r1, [r0, #0x100]        /* for S1 */
-        str r1, [r0, #0x200]        /* for S2 */
-        str r1, [r0, #0x300]        /* for S3 */
-        str r1, [r0, #0x400]        /* for S4 */
-        /* SGPCR - always park on last master */
-        ldr r1, =0x10
-        str r1, [r0, #0x010]        /* for S0 */
-        str r1, [r0, #0x110]        /* for S1 */
-        str r1, [r0, #0x210]        /* for S2 */
-        str r1, [r0, #0x310]        /* for S3 */
-        str r1, [r0, #0x410]        /* for S4 */
-        /* MGPCR - restore default values */
-        ldr r1, =0x0
-        str r1, [r0, #0x800]        /* for M0 */
-        str r1, [r0, #0x900]        /* for M1 */
-        str r1, [r0, #0xA00]        /* for M2 */
-        str r1, [r0, #0xB00]        /* for M3 */
-        str r1, [r0, #0xC00]        /* for M4 */
-        str r1, [r0, #0xD00]        /* for M5 */
-
-	ldr r1, M3IF_BASE_W
 	/*
-	 * M3IF Control Register (M3IFCTL)
-	 * MRRP[0] = L2CC0 not on priority list (0 << 0)	= 0x00000000
-	 * MRRP[1] = MAX1 not on priority list (0 << 0)		= 0x00000000
-	 * MRRP[2] = L2CC1 not on priority list (0 << 0)	= 0x00000000
-	 * MRRP[3] = USB  not on priority list (0 << 0)		= 0x00000000
-	 * MRRP[4] = SDMA not on priority list (0 << 0)		= 0x00000000
-	 * MRRP[5] = GPU not on priority list (0 << 0)		= 0x00000000
-	 * MRRP[6] = IPU1 on priority list (1 << 6)		= 0x00000040
-	 * MRRP[7] = IPU2 not on priority list (0 << 0)		= 0x00000000
-	 *                                                       ------------
-	 *                                                        0x00000040
+	 * Branch predicition is now enabled.  Flush the BTAC to ensure a valid
+	 * starting point.  Don't flush BTAC while it is disabled to avoid
+	 * ARM1136 erratum 408023.
 	 */
-	ldr r0, =0x00000040
-	str r0, [r1]  /* M3IF control reg */
+	mov     r0, #0
+	mcr     p15, 0, r0, c7, c5, 6	/* flush entire BTAC */
 
-#if 1
-	ldr r0, CCM_BASE_ADDR_W
+	mov r0, #0
+	mcr 15, 0, r0, c7, c7, 0        /* invalidate I cache and D cache */
+	mcr 15, 0, r0, c8, c7, 0        /* invalidate TLBs */
+	mcr 15, 0, r0, c7, c10, 4       /* Drain the write buffer */
+
+	/* Also setup the Peripheral Port Remap register inside the core */
+	ldr r0, ARM_PPMRR        /* start from AIPS 2GB region */
+	mcr p15, 0, r0, c15, c2, 4
 
-	/* default CLKO to 1/32 of the ARM core*/
-	ldr r1, [r0, #CCM_COSR]
-	bic r1, r1, #0x00000FF00
-	bic r1, r1, #0x0000000FF
-	mov r2, #0x00006C00
-	add r2, r2, #0x67
-	orr r1, r1, r2
-	str r1, [r0, #CCM_COSR]
+/*
+ * End of ARM1136 init
+ */
+	ldr r0, CCM_BASE_ADDR_W
 
 	ldr r2, CCM_CCMR_W
 	str r2, [r0, #CCM_CCMR]
 
-	/* check clock path */
-	ldr r2, [r0, #CCM_PDR0]
-	tst r2, #0x1
-	ldrne r3, MPCTL_PARAM_532_W  /* consumer path*/
-	ldreq r3, MPCTL_PARAM_399_W  /* auto path*/
+	ldr r3, MPCTL_PARAM_532_W  /* consumer path*/
 
-	/*Set MPLL , arm clock and ahb clock*/
+	/* Set MPLL , arm clock and ahb clock*/
 	str r3, [r0, #CCM_MPCTL]
 
 	ldr r1, PPCTL_PARAM_W
 	str r1, [r0, #CCM_PPCTL]
 
-	ldr r1, [r0, #CCM_PDR0]
-	orr r1, r1, #0x800000
-	str r1, [r0, #CCM_PDR0]
-
 	ldr r1, CCM_PDR0_W
 	str r1, [r0, #CCM_PDR0]
 
@@ -302,7 +120,6 @@ L2_loop:
 	orr r1, r1, #0x00000C00
 	orr r1, r1, #0x00000003
 	str r1, [r0, #CCM_CGR1]
-#endif
 
 	/* Skip SDRAM initialization if we run from RAM */
 	cmp	pc, #0x80000000
@@ -310,49 +127,69 @@ L2_loop:
 	cmp	pc, #0x90000000
 	bhi	1f
 
-	mov	pc, lr
+	mov	pc, r10
 
 1:
-	ldr r0, ESDCTL_BASE_W
+	ldr r0, =ESDCTL_BASE_ADDR
 	mov r3, #0x2000
 	str r3, [r0, #0x0]
 	str r3, [r0, #0x8]
 
-	mov r12, #0x00
-	mov r2, #0x00
-	mov r1, #IMX_SDRAM_CS0
+	/* ip(r12) has used to save lr register in upper calling*/
+	mov fp, lr
 
-	ldr r0, ESDCTL_BASE_W
-	mov r3, #0x2000
-	str r3, [r0, #0x0]
-	str r3, [r0, #0x8]
-
-	mov r12, #0x00
+	mov r5, #0x00
 	mov r2, #0x00
-	mov r1, #IMX_SDRAM_CS0
+	mov r1, #CSD0_BASE_ADDR
 	bl setup_sdram_bank
 	cmp r3, #0x0
-	orreq r12, r12, #1
+	orreq r5, r5, #1
 	eorne r2, r2, #0x1
 	blne setup_sdram_bank
 
-	cmp r12, #0
-	movne r3, #L2CC_BASE_ADDR
-	ldrne r4, [r3, #L2_CACHE_AUX_CTL_REG]
-	orrne r4, r4, #0x1000
-	strne r4, [r3, #L2_CACHE_AUX_CTL_REG]
+	mov lr, fp
 
-	ldr r3, ESDCTL_DELAY5
+	ldr r3, =ESDCTL_DELAY_LINE5
 	str r3, [r0, #0x30]
+
+#ifdef CONFIG_NAND_IMX_BOOT
+	ldr	sp, =TEXT_BASE - 4	/* Setup a temporary stack in SDRAM */
+
+	ldr	r0, =IMX_NFC_BASE		/* start of NFC SRAM                */
+	ldr	r2, =IMX_NFC_BASE + 0x800	/* end of NFC SRAM                  */
+
+	/* skip NAND boot if not running from NFC space */
+	cmp	pc, r0
+	blo	ret
+	cmp	pc, r2
+	bhs	ret
+
+	/* Move ourselves out of NFC SRAM */
+	ldr	r1, =TEXT_BASE
+
+copy_loop:
+	ldmia	r0!, {r3-r9}		/* copy from source address [r0]    */
+	stmia	r1!, {r3-r9}		/* copy to   target address [r1]    */
+	cmp	r0, r2			/* until source end addreee [r2]    */
+	ble	copy_loop
+
+	ldr	pc, =1f			/* Jump to SDRAM                    */
+1:
+	bl	nand_boot		/* Load U-Boot from NAND Flash      */
+
+	/* rebase the return address */
+	ldr	r1, =IMX_NFC_BASE - TEXT_BASE
+	sub	r10, r10, r1		/* adjust return address from NFC SRAM */
 ret:
-	mov	pc,r10
+#endif /* CONFIG_NAND_IMX_BOOT */
+
+	mov pc, r10
 
 /*
- * r0: control base, r1: ram bank base
- * r2: ddr type(0:DDR2, 1:MDDR) r3, r4: working
+ * r0: ESDCTL control base, r1: sdram slot base
+ * r2: DDR type(0:DDR2, 1:MDDR) r3, r4:working base
  */
 setup_sdram_bank:
-
 	mov r3, #0xE /*0xA + 0x4*/
 	tst r2, #0x1
 	orreq r3, r3, #0x300 /*DDR2*/
@@ -365,74 +202,71 @@ setup_sdram_bank:
 1:	subs r3, r3, #1
 	bne 1b
 
-2:      adr r4, ESDCTL_CONFIG
-	tst r2, #0x1
-	ldreq r3, [r4, #0x0]
-	ldrne r3, [r4, #0x4]
-	cmp r1, #IMX_SDRAM_CS1
-        strlo r3, [r0, #0x4]
-        strhs r3, [r0, #0xC]
-
-        ldr r3, ESDCTL_0x92220000
-        strlo r3, [r0, #0x0]
-        strhs r3, [r0, #0x8]
+2:	tst r2, #0x1
+	ldreq r3, =ESDCTL_DDR2_CONFIG
+	ldrne r3, =ESDCTL_MDDR_CONFIG
+	cmp r1, #CSD1_BASE_ADDR
+	strlo r3, [r0, #0x4]
+	strhs r3, [r0, #0xC]
+
+	ldr r3, =ESDCTL_0x92220000
+	strlo r3, [r0, #0x0]
+	strhs r3, [r0, #0x8]
 	mov r3, #0xDA
-        ldr r4, RAM_PARAM1_MDDR
-        strb r3, [r1, r4]
+	ldr r4, =ESDCTL_PRECHARGE
+	strb r3, [r1, r4]
 
 	tst r2, #0x1
 	bne skip_set_mode
 
-	cmp r1, #IMX_SDRAM_CS1
-	ldr r3, ESDCTL_0xB2220000
-        strlo r3, [r0, #0x0]
-        strhs r3, [r0, #0x8]
+	cmp r1, #CSD1_BASE_ADDR
+	ldr r3, =ESDCTL_0xB2220000
+	strlo r3, [r0, #0x0]
+	strhs r3, [r0, #0x8]
 	mov r3, #0xDA
-	ldr r4, RAM_PARAM4_MDDR
-        strb r3, [r1, r4]
-        ldr r4, RAM_PARAM5_MDDR
-        strb r3, [r1, r4]
-        ldr r4, RAM_PARAM3_MDDR
-        strb r3, [r1, r4]
-        ldr r4, RAM_PARAM2_MDDR
-        strb r3, [r1, r4]
-
-        ldr r3, ESDCTL_0x92220000
-        strlo r3, [r0, #0x0]
-        strhs r3, [r0, #0x8]
+	ldr r4, =ESDCTL_DDR2_EMR2
+	strb r3, [r1, r4]
+	ldr r4, =ESDCTL_DDR2_EMR3
+	strb r3, [r1, r4]
+	ldr r4, =ESDCTL_DDR2_EN_DLL
+	strb r3, [r1, r4]
+	ldr r4, =ESDCTL_DDR2_RESET_DLL
+	strb r3, [r1, r4]
+
+	ldr r3, =ESDCTL_0x92220000
+	strlo r3, [r0, #0x0]
+	strhs r3, [r0, #0x8]
 	mov r3, #0xDA
-        ldr r4, RAM_PARAM1_MDDR
-        strb r3, [r1, r4]
+	ldr r4, =ESDCTL_PRECHARGE
+	strb r3, [r1, r4]
 
 skip_set_mode:
-	cmp r1, #IMX_SDRAM_CS1
-        ldr r3, ESDCTL_0xA2220000
-        strlo r3, [r0, #0x0]
-        strhs r3, [r0, #0x8]
-        mov r3, #0xDA
-        strb r3, [r1]
-        strb r3, [r1]
-
-        ldr r3, ESDCTL_0xB2220000
-        strlo r3, [r0, #0x0]
-        strhs r3, [r0, #0x8]
-	adr r4, RAM_PARAM6_MDDR
+	cmp r1, #CSD1_BASE_ADDR
+	ldr r3, =ESDCTL_0xA2220000
+	strlo r3, [r0, #0x0]
+	strhs r3, [r0, #0x8]
+	mov r3, #0xDA
+	strb r3, [r1]
+	strb r3, [r1]
+
+	ldr r3, =ESDCTL_0xB2220000
+	strlo r3, [r0, #0x0]
+	strhs r3, [r0, #0x8]
 	tst r2, #0x1
-	ldreq r4, [r4, #0x0]
-	ldrne r4, [r4, #0x4]
-        mov r3, #0xDA
-        strb r3, [r1, r4]
-        ldreq r4, RAM_PARAM7_MDDR
-        streqb r3, [r1, r4]
-	adr r4, RAM_PARAM3_MDDR
-	ldreq r4, [r4, #0x0]
-	ldrne r4, [r4, #0x4]
-        strb r3, [r1, r4]
-
-	cmp r1, #IMX_SDRAM_CS1
-        ldr r3, ESDCTL_0x82226080
-        strlo r3, [r0, #0x0]
-        strhs r3, [r0, #0x8]
+	ldreq r4, =ESDCTL_DDR2_MR
+	ldrne r4, =ESDCTL_MDDR_MR
+	mov r3, #0xDA
+	strb r3, [r1, r4]
+	ldreq r4, =ESDCTL_DDR2_OCD_DEFAULT
+	streqb r3, [r1, r4]
+	ldreq r4, =ESDCTL_DDR2_EN_DLL
+	ldrne r4, =ESDCTL_MDDR_EMR
+	strb r3, [r1, r4]
+
+	cmp r1, #CSD1_BASE_ADDR
+	ldr r3, =ESDCTL_0x82228080
+	strlo r3, [r0, #0x0]
+	strhs r3, [r0, #0x8]
 
 	tst r2, #0x1
 	moveq r4, #0x20000
@@ -445,5 +279,5 @@ skip_set_mode:
 	cmp r3, r4
 	movne r3, #1
 	moveq r3, #0
-	mov pc, lr
 
+	mov pc, lr