ENGR00154672: Change to use rom plugins for mx53 boards

1. As customer required, we change to use rom plugins for mx53 boards.
Tested pass with latest mfg tools.
2. Update DDR3 script based on MX53_TO2_DDR3_LCB_SMD_ARDb_v2.inc.
Got from
http://compass.freescale.net/livelink/livelink?func=ll
&objId=221058910&objAction=browse&viewType=1.
3. Fix a tiny build error in mx53_smd.c.
This error will happen when building mx53_smd_mfg.

Signed-off-by: Terry Lv <r65388@freescale.com>

5 files changed, 889 insertions, 465 deletions

diff --git a/board/freescale/mx53_ard/flash_header.S b/board/freescale/mx53_ard/flash_header.S
index 94348bb..a85870c 100644
--- a/board/freescale/mx53_ard/flash_header.S
+++ b/board/freescale/mx53_ard/flash_header.S
@@ -20,166 +20,288 @@
 #include <config.h>
 #include <asm/arch/mx53.h>
 
+#define REG_LD_AND_STR_INIT(base)	\
+	ldr r0, =base;
+
+#define REG_LD_AND_STR_OP(i, offset, val)	\
+	ldr r1, =val;	\
+	ldr r2, =offset;	\
+	str r1, [r0, r2];
+
+#define REG_LD_AND_STR_END(base)
+
+
 #ifdef	CONFIG_FLASH_HEADER
 #ifndef CONFIG_FLASH_HEADER_OFFSET
 # error "Must define the offset of flash header"
 #endif
 
-#define CPU_2_BE_32(l) \
-       ((((l) & 0x000000FF) << 24) | \
-	(((l) & 0x0000FF00) << 8)  | \
-	(((l) & 0x00FF0000) >> 8)  | \
-	(((l) & 0xFF000000) >> 24))
-
-#define MXC_DCD_ITEM(i, addr, val)   \
-dcd_node_##i:                        \
-        .word CPU_2_BE_32(addr) ;     \
-        .word CPU_2_BE_32(val)  ;     \
-
 .section ".text.flasheader", "x"
 	b	_start
 	.org	CONFIG_FLASH_HEADER_OFFSET
-ivt_header:       .word 0x402000D1 /* Tag=0xD1, Len=0x0020, Ver=0x40 */
-app_code_jump_v:  .word _start
-reserv1:          .word 0x0
-dcd_ptr:          .word dcd_hdr
-boot_data_ptr:	  .word boot_data
-self_ptr:         .word ivt_header
-app_code_csf:     .word 0x0
-reserv2:          .word 0x0
-
-boot_data:        .word 0x77800000
-image_len:        .word _end_of_copy  - TEXT_BASE + CONFIG_FLASH_HEADER_OFFSET
-plugin:           .word 0x0
-
-/* TO1.0/TO2.0 DDR2 scripts for EVK, ARD and ARM2 CPU2 board */
-#if !defined(CONFIG_MX53_ARD_DDR3)
-dcd_hdr:          .word 0x400802D2 /* Tag=0xD2, Len=64*8 + 4 + 4, Ver=0x40 */
-write_dcd_cmd:    .word 0x040402CC /* Tag=0xCC, Len=64*8 + 4, Param=4 */
-
-/* DCD */
-MXC_DCD_ITEM(1, IOMUXC_BASE_ADDR + 0x554, 0x00200000)
-MXC_DCD_ITEM(2, IOMUXC_BASE_ADDR + 0x560, 0x00200000)
-MXC_DCD_ITEM(3, IOMUXC_BASE_ADDR + 0x594, 0x00200000)
-MXC_DCD_ITEM(4, IOMUXC_BASE_ADDR + 0x584, 0x00200000)
-MXC_DCD_ITEM(5, IOMUXC_BASE_ADDR + 0x558, 0x00200040)
-MXC_DCD_ITEM(6, IOMUXC_BASE_ADDR + 0x568, 0x00200040)
-MXC_DCD_ITEM(7, IOMUXC_BASE_ADDR + 0x590, 0x00200040)
-MXC_DCD_ITEM(8, IOMUXC_BASE_ADDR + 0x57c, 0x00200040)
-MXC_DCD_ITEM(9, IOMUXC_BASE_ADDR + 0x564, 0x00200040)
-MXC_DCD_ITEM(10, IOMUXC_BASE_ADDR + 0x580, 0x00200040)
-MXC_DCD_ITEM(11, IOMUXC_BASE_ADDR + 0x570, 0x00200000)
-MXC_DCD_ITEM(12, IOMUXC_BASE_ADDR + 0x578, 0x00200000)
-MXC_DCD_ITEM(13, IOMUXC_BASE_ADDR + 0x72c, 0x00200000)
-MXC_DCD_ITEM(14, IOMUXC_BASE_ADDR + 0x728, 0x00200000)
-MXC_DCD_ITEM(15, IOMUXC_BASE_ADDR + 0x71c, 0x00200000)
-MXC_DCD_ITEM(16, IOMUXC_BASE_ADDR + 0x718, 0x00200000)
-MXC_DCD_ITEM(17, IOMUXC_BASE_ADDR + 0x574, 0x00280000)
-MXC_DCD_ITEM(18, IOMUXC_BASE_ADDR + 0x588, 0x00280000)
-MXC_DCD_ITEM(19, IOMUXC_BASE_ADDR + 0x6f0, 0x00280000)
-MXC_DCD_ITEM(20, IOMUXC_BASE_ADDR + 0x720, 0x00280000)
-MXC_DCD_ITEM(21, IOMUXC_BASE_ADDR + 0x6fc, 0x00000000)
-MXC_DCD_ITEM(22, IOMUXC_BASE_ADDR + 0x6f4, 0x00000200)
-MXC_DCD_ITEM(23, IOMUXC_BASE_ADDR + 0x714, 0x00000000)
-MXC_DCD_ITEM(24, IOMUXC_BASE_ADDR + 0x724, 0x06000000)
-MXC_DCD_ITEM(25, ESDCTL_BASE_ADDR + 0x088, 0x34333936)
-MXC_DCD_ITEM(26, ESDCTL_BASE_ADDR + 0x090, 0x49434942)
-MXC_DCD_ITEM(27, ESDCTL_BASE_ADDR + 0x0f8, 0x00000800)
-MXC_DCD_ITEM(28, ESDCTL_BASE_ADDR + 0x07c, 0x01350138)
-MXC_DCD_ITEM(29, ESDCTL_BASE_ADDR + 0x080, 0x01380139)
-MXC_DCD_ITEM(30, ESDCTL_BASE_ADDR + 0x018, 0x00001710)
-MXC_DCD_ITEM(31, ESDCTL_BASE_ADDR + 0x000, 0xc4110000)
-MXC_DCD_ITEM(32, ESDCTL_BASE_ADDR + 0x00c, 0x4d5122d2)
-MXC_DCD_ITEM(33, ESDCTL_BASE_ADDR + 0x010, 0x92d18a22)
-MXC_DCD_ITEM(34, ESDCTL_BASE_ADDR + 0x014, 0x00c70092)
-MXC_DCD_ITEM(35, ESDCTL_BASE_ADDR + 0x02c, 0x000026d2)
-MXC_DCD_ITEM(36, ESDCTL_BASE_ADDR + 0x030, 0x009f000e)
-MXC_DCD_ITEM(37, ESDCTL_BASE_ADDR + 0x008, 0x12272000)
-MXC_DCD_ITEM(38, ESDCTL_BASE_ADDR + 0x004, 0x00030012)
-MXC_DCD_ITEM(39, ESDCTL_BASE_ADDR + 0x01c, 0x04008010)
-MXC_DCD_ITEM(40, ESDCTL_BASE_ADDR + 0x01c, 0x00008032)
-MXC_DCD_ITEM(41, ESDCTL_BASE_ADDR + 0x01c, 0x00008033)
-MXC_DCD_ITEM(42, ESDCTL_BASE_ADDR + 0x01c, 0x00008031)
-MXC_DCD_ITEM(43, ESDCTL_BASE_ADDR + 0x01c, 0x0b5280b0)
-MXC_DCD_ITEM(44, ESDCTL_BASE_ADDR + 0x01c, 0x04008010)
-MXC_DCD_ITEM(45, ESDCTL_BASE_ADDR + 0x01c, 0x00008020)
-MXC_DCD_ITEM(46, ESDCTL_BASE_ADDR + 0x01c, 0x00008020)
-MXC_DCD_ITEM(47, ESDCTL_BASE_ADDR + 0x01c, 0x0a528030)
-MXC_DCD_ITEM(48, ESDCTL_BASE_ADDR + 0x01c, 0x03c68031)
-MXC_DCD_ITEM(49, ESDCTL_BASE_ADDR + 0x01c, 0x00448031)
-MXC_DCD_ITEM(50, ESDCTL_BASE_ADDR + 0x01c, 0x04008018)
-MXC_DCD_ITEM(51, ESDCTL_BASE_ADDR + 0x01c, 0x0000803a)
-MXC_DCD_ITEM(52, ESDCTL_BASE_ADDR + 0x01c, 0x0000803b)
-MXC_DCD_ITEM(53, ESDCTL_BASE_ADDR + 0x01c, 0x00008039)
-MXC_DCD_ITEM(54, ESDCTL_BASE_ADDR + 0x01c, 0x0b528138)
-MXC_DCD_ITEM(55, ESDCTL_BASE_ADDR + 0x01c, 0x04008018)
-MXC_DCD_ITEM(56, ESDCTL_BASE_ADDR + 0x01c, 0x00008028)
-MXC_DCD_ITEM(57, ESDCTL_BASE_ADDR + 0x01c, 0x00008028)
-MXC_DCD_ITEM(58, ESDCTL_BASE_ADDR + 0x01c, 0x0a528038)
-MXC_DCD_ITEM(59, ESDCTL_BASE_ADDR + 0x01c, 0x03c68039)
-MXC_DCD_ITEM(60, ESDCTL_BASE_ADDR + 0x01c, 0x00448039)
-MXC_DCD_ITEM(61, ESDCTL_BASE_ADDR + 0x020, 0x00005800)
-MXC_DCD_ITEM(62, ESDCTL_BASE_ADDR + 0x058, 0x00033335)
-MXC_DCD_ITEM(63, ESDCTL_BASE_ADDR + 0x01c, 0x00000000)
-MXC_DCD_ITEM(64, ESDCTL_BASE_ADDR + 0x040, 0x04b80003)
-
-
-#elif defined(CONFIG_MX53_ARD_DDR3)
-dcd_hdr:          .word 0x40A001D2 /* Tag=0xD2, Len=51*8 + 4 + 4, Ver=0x40 */
-write_dcd_cmd:    .word 0x049C01CC /* Tag=0xCC, Len=51*8 + 4, Param=4 */
-
-/* DCD */
-MXC_DCD_ITEM(1, IOMUXC_BASE_ADDR + 0x554, 0x00300000)
-MXC_DCD_ITEM(2, IOMUXC_BASE_ADDR + 0x558, 0x00300040)
-MXC_DCD_ITEM(3, IOMUXC_BASE_ADDR + 0x560, 0x00300000)
-MXC_DCD_ITEM(4, IOMUXC_BASE_ADDR + 0x564, 0x00300040)
-MXC_DCD_ITEM(5, IOMUXC_BASE_ADDR + 0x568, 0x00300040)
-MXC_DCD_ITEM(6, IOMUXC_BASE_ADDR + 0x570, 0x00300000)
-MXC_DCD_ITEM(7, IOMUXC_BASE_ADDR + 0x574, 0x00300000)
-MXC_DCD_ITEM(8, IOMUXC_BASE_ADDR + 0x578, 0x00300000)
-MXC_DCD_ITEM(9, IOMUXC_BASE_ADDR + 0x57c, 0x00300040)
-MXC_DCD_ITEM(10, IOMUXC_BASE_ADDR + 0x580, 0x00300040)
-MXC_DCD_ITEM(11, IOMUXC_BASE_ADDR + 0x584, 0x00300000)
-MXC_DCD_ITEM(12, IOMUXC_BASE_ADDR + 0x588, 0x00300000)
-MXC_DCD_ITEM(13, IOMUXC_BASE_ADDR + 0x590, 0x00300040)
-MXC_DCD_ITEM(14, IOMUXC_BASE_ADDR + 0x594, 0x00300000)
-MXC_DCD_ITEM(15, IOMUXC_BASE_ADDR + 0x6f0, 0x00300000)
-MXC_DCD_ITEM(16, IOMUXC_BASE_ADDR + 0x6f4, 0x00000000)
-MXC_DCD_ITEM(17, IOMUXC_BASE_ADDR + 0x6fc, 0x00000000)
-MXC_DCD_ITEM(18, IOMUXC_BASE_ADDR + 0x714, 0x00000000)
-MXC_DCD_ITEM(19, IOMUXC_BASE_ADDR + 0x718, 0x00300000)
-MXC_DCD_ITEM(20, IOMUXC_BASE_ADDR + 0x71c, 0x00300000)
-MXC_DCD_ITEM(21, IOMUXC_BASE_ADDR + 0x720, 0x00300000)
-MXC_DCD_ITEM(22, IOMUXC_BASE_ADDR + 0x724, 0x04000000)
-MXC_DCD_ITEM(23, IOMUXC_BASE_ADDR + 0x728, 0x00300000)
-MXC_DCD_ITEM(24, IOMUXC_BASE_ADDR + 0x72c, 0x00300000)
-MXC_DCD_ITEM(25, ESDCTL_BASE_ADDR + 0x088, 0x35343535)
-MXC_DCD_ITEM(26, ESDCTL_BASE_ADDR + 0x090, 0x4d444c44)
-MXC_DCD_ITEM(27, ESDCTL_BASE_ADDR + 0x07c, 0x01370138)
-MXC_DCD_ITEM(28, ESDCTL_BASE_ADDR + 0x080, 0x013b013c)
-MXC_DCD_ITEM(29, ESDCTL_BASE_ADDR + 0x018, 0x00011740)
-MXC_DCD_ITEM(30, ESDCTL_BASE_ADDR + 0x000, 0xc3190000)
-MXC_DCD_ITEM(31, ESDCTL_BASE_ADDR + 0x00c, 0x9f5152e3)
-MXC_DCD_ITEM(32, ESDCTL_BASE_ADDR + 0x010, 0xb68e8a63)
-MXC_DCD_ITEM(33, ESDCTL_BASE_ADDR + 0x014, 0x01ff00db)
-MXC_DCD_ITEM(34, ESDCTL_BASE_ADDR + 0x02c, 0x000026d2)
-MXC_DCD_ITEM(35, ESDCTL_BASE_ADDR + 0x030, 0x009f0e21)
-MXC_DCD_ITEM(36, ESDCTL_BASE_ADDR + 0x008, 0x12273030)
-MXC_DCD_ITEM(37, ESDCTL_BASE_ADDR + 0x004, 0x0002002d)
-MXC_DCD_ITEM(38, ESDCTL_BASE_ADDR + 0x01c, 0x00008032)
-MXC_DCD_ITEM(39, ESDCTL_BASE_ADDR + 0x01c, 0x00008033)
-MXC_DCD_ITEM(40, ESDCTL_BASE_ADDR + 0x01c, 0x00028031)
-MXC_DCD_ITEM(41, ESDCTL_BASE_ADDR + 0x01c, 0x052080b0)
-MXC_DCD_ITEM(42, ESDCTL_BASE_ADDR + 0x01c, 0x04008040)
-MXC_DCD_ITEM(43, ESDCTL_BASE_ADDR + 0x01c, 0x0000803a)
-MXC_DCD_ITEM(44, ESDCTL_BASE_ADDR + 0x01c, 0x0000803b)
-MXC_DCD_ITEM(45, ESDCTL_BASE_ADDR + 0x01c, 0x00028039)
-MXC_DCD_ITEM(46, ESDCTL_BASE_ADDR + 0x01c, 0x05208138)
-MXC_DCD_ITEM(47, ESDCTL_BASE_ADDR + 0x01c, 0x04008048)
-MXC_DCD_ITEM(48, ESDCTL_BASE_ADDR + 0x020, 0x00005800)
-MXC_DCD_ITEM(49, ESDCTL_BASE_ADDR + 0x040, 0x04b80003)
-MXC_DCD_ITEM(50, ESDCTL_BASE_ADDR + 0x058, 0x00022227)
-MXC_DCD_ITEM(51, ESDCTL_BASE_ADDR + 0x01C, 0x00000000)
 
+ivt_header:		.long 0x402000D1/* Tag=0xD1, Len=0x0020, Ver=0x40 */
+app_code_jump_v:	.long (0xF8006000 + (plugin_start - TEXT_BASE))
+reserv1:		.long 0x0
+dcd_ptr:		.long 0x0
+boot_data_ptr:		.long (0xF8006000 + (boot_data - TEXT_BASE))
+self_ptr:		.long (0xF8006000 + (ivt_header - TEXT_BASE))
+app_code_csf:		.long 0x0
+reserv2:		.long 0x0
+boot_data:		.long 0xF8006000
+image_len:		.long (3 * 1024)
+plugin:			.long 0x1
+
+/* Second IVT to give entry point into the bootloader copied to DDR */
+ivt2_header:		.long 0x402000D1/*Tag=0xD1, Len=0x0020, Ver=0x40 */
+app2_code_jump_v:	.long _start   /* Entry point for the bootloader */
+reserv3:		.long 0x0
+dcd2_ptr:		.long 0x0
+boot_data2_ptr:		.long boot_data2
+self_ptr2:		.long ivt2_header
+app_code_csf2:		.long 0x0
+reserv4:		.long 0x0
+boot_data2:		.long TEXT_BASE
+image_len2:		.long  _end_of_copy - TEXT_BASE + CONFIG_FLASH_HEADER_OFFSET
+plugin2:		.long 0x0
+
+/* Here starts the plugin code */
+plugin_start:
+	/* Save the return address and the function arguments */
+	push {r0-r6, lr}
+	/* We should distinguish USB recovery mode(SDP mode) and internal boot mode.
+	If ROM runs in SDP mode, then it needn't load boot code from storage media.
+	If ROM runs in SDP mode, then r0 must be 0x00
+	If ROM runs in internal boot mode, then r0 should be larger than IRAM base address. */
+	mov r7, r0
+
+#if defined(CONFIG_MX53_ARD_DDR3)
+	/* IOMUX */
+	REG_LD_AND_STR_INIT(IOMUXC_BASE_ADDR)
+	REG_LD_AND_STR_OP(1, 0x554, 0x00300000)
+	REG_LD_AND_STR_OP(2, 0x558, 0x00300040)
+	REG_LD_AND_STR_OP(3, 0x560, 0x00300000)
+	REG_LD_AND_STR_OP(4, 0x564, 0x00300040)
+	REG_LD_AND_STR_OP(5, 0x568, 0x00300040)
+	REG_LD_AND_STR_OP(6, 0x570, 0x00300000)
+	REG_LD_AND_STR_OP(7, 0x574, 0x00300000)
+	REG_LD_AND_STR_OP(8, 0x578, 0x00300000)
+	REG_LD_AND_STR_OP(9, 0x57c, 0x00300040)
+	REG_LD_AND_STR_OP(10, 0x580, 0x00300040)
+	REG_LD_AND_STR_OP(11, 0x584, 0x00300000)
+	REG_LD_AND_STR_OP(12, 0x588, 0x00300000)
+	REG_LD_AND_STR_OP(13, 0x590, 0x00300040)
+	REG_LD_AND_STR_OP(14, 0x594, 0x00300000)
+	REG_LD_AND_STR_OP(15, 0x6f0, 0x00300000)
+	REG_LD_AND_STR_OP(16, 0x6f4, 0x00000000)
+	REG_LD_AND_STR_OP(17, 0x6fc, 0x00000000)
+	REG_LD_AND_STR_OP(18, 0x714, 0x00000000)
+	REG_LD_AND_STR_OP(19, 0x718, 0x00300000)
+	REG_LD_AND_STR_OP(20, 0x71c, 0x00300000)
+	REG_LD_AND_STR_OP(21, 0x720, 0x00300000)
+	REG_LD_AND_STR_OP(22, 0x724, 0x04000000)
+	REG_LD_AND_STR_OP(23, 0x728, 0x00300000)
+	REG_LD_AND_STR_OP(24, 0x72c, 0x00300000)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+	/* ESDCTL */
+	REG_LD_AND_STR_INIT(ESDCTL_BASE_ADDR)
+	REG_LD_AND_STR_OP(25, 0x088, 0x35343535)
+	REG_LD_AND_STR_OP(26, 0x090, 0x4d444c44)
+	REG_LD_AND_STR_OP(27, 0x07c, 0x01370138)
+	REG_LD_AND_STR_OP(28, 0x080, 0x013b013c)
+	REG_LD_AND_STR_OP(29, 0x018, 0x00001740)
+	REG_LD_AND_STR_OP(30, 0x000, 0xc3190000)
+	REG_LD_AND_STR_OP(31, 0x00c, 0x9f5152e3)
+	REG_LD_AND_STR_OP(32, 0x010, 0xb68e8a63)
+	REG_LD_AND_STR_OP(33, 0x014, 0x01ff00db)
+	REG_LD_AND_STR_OP(34, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(35, 0x030, 0x009f0e21)
+	REG_LD_AND_STR_OP(36, 0x008, 0x12273030)
+	REG_LD_AND_STR_OP(37, 0x004, 0x0002002d)
+	REG_LD_AND_STR_OP(38, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(39, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x00028031)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x052080b0)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x04008040)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x00028039)
+	REG_LD_AND_STR_OP(46, 0x01c, 0x05208138)
+	REG_LD_AND_STR_OP(47, 0x01c, 0x04008048)
+	REG_LD_AND_STR_OP(48, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(49, 0x040, 0x04b80003)
+	REG_LD_AND_STR_OP(50, 0x058, 0x00022227)
+	REG_LD_AND_STR_OP(51, 0x01C, 0x00000000)
+	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
+#else
+	/* IOMUX */
+	REG_LD_AND_STR_INIT(IOMUXC_BASE_ADDR)
+	REG_LD_AND_STR_OP(1, 0x554, 0x00200000)
+	REG_LD_AND_STR_OP(2, 0x560, 0x00200000)
+	REG_LD_AND_STR_OP(3, 0x594, 0x00200000)
+	REG_LD_AND_STR_OP(4, 0x584, 0x00200000)
+	REG_LD_AND_STR_OP(5, 0x558, 0x00200040)
+	REG_LD_AND_STR_OP(6, 0x568, 0x00200040)
+	REG_LD_AND_STR_OP(7, 0x590, 0x00200040)
+	REG_LD_AND_STR_OP(8, 0x57c, 0x00200040)
+	REG_LD_AND_STR_OP(9, 0x564, 0x00200040)
+	REG_LD_AND_STR_OP(10, 0x580, 0x00200040)
+	REG_LD_AND_STR_OP(11, 0x570, 0x00200000)
+	REG_LD_AND_STR_OP(12, 0x578, 0x00200000)
+	REG_LD_AND_STR_OP(13, 0x72c, 0x00200000)
+	REG_LD_AND_STR_OP(14, 0x728, 0x00200000)
+	REG_LD_AND_STR_OP(15, 0x71c, 0x00200000)
+	REG_LD_AND_STR_OP(16, 0x718, 0x00200000)
+	REG_LD_AND_STR_OP(17, 0x574, 0x00280000)
+	REG_LD_AND_STR_OP(18, 0x588, 0x00280000)
+	REG_LD_AND_STR_OP(19, 0x6f0, 0x00280000)
+	REG_LD_AND_STR_OP(20, 0x720, 0x00280000)
+	REG_LD_AND_STR_OP(21, 0x6fc, 0x00000000)
+	REG_LD_AND_STR_OP(22, 0x6f4, 0x00000200)
+	REG_LD_AND_STR_OP(23, 0x714, 0x00000000)
+	REG_LD_AND_STR_OP(24, 0x724, 0x06000000)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+	/* ESDCTL */
+	REG_LD_AND_STR_INIT(ESDCTL_BASE_ADDR)
+	REG_LD_AND_STR_OP(25, 0x088, 0x34333936)
+	REG_LD_AND_STR_OP(26, 0x090, 0x49434942)
+	REG_LD_AND_STR_OP(27, 0x0f8, 0x00000800)
+	REG_LD_AND_STR_OP(28, 0x07c, 0x01350138)
+	REG_LD_AND_STR_OP(29, 0x080, 0x01380139)
+	REG_LD_AND_STR_OP(30, 0x018, 0x00001710)
+	REG_LD_AND_STR_OP(31, 0x000, 0xc4110000)
+	REG_LD_AND_STR_OP(32, 0x00c, 0x4d5122d2)
+	REG_LD_AND_STR_OP(33, 0x010, 0x92d18a22)
+	REG_LD_AND_STR_OP(34, 0x014, 0x00c70092)
+	REG_LD_AND_STR_OP(35, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(36, 0x030, 0x009f000e)
+	REG_LD_AND_STR_OP(37, 0x008, 0x12272000)
+	REG_LD_AND_STR_OP(38, 0x004, 0x00030012)
+	REG_LD_AND_STR_OP(39, 0x01c, 0x04008010)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x00008031)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x0b5280b0)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x04008010)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x00008020)
+	REG_LD_AND_STR_OP(46, 0x01c, 0x00008020)
+	REG_LD_AND_STR_OP(47, 0x01c, 0x0a528030)
+	REG_LD_AND_STR_OP(48, 0x01c, 0x03c68031)
+	REG_LD_AND_STR_OP(49, 0x01c, 0x00448031)
+	REG_LD_AND_STR_OP(50, 0x01c, 0x04008018)
+	REG_LD_AND_STR_OP(51, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(52, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(53, 0x01c, 0x00008039)
+	REG_LD_AND_STR_OP(54, 0x01c, 0x0b528138)
+	REG_LD_AND_STR_OP(55, 0x01c, 0x04008018)
+	REG_LD_AND_STR_OP(56, 0x01c, 0x00008028)
+	REG_LD_AND_STR_OP(57, 0x01c, 0x00008028)
+	REG_LD_AND_STR_OP(58, 0x01c, 0x0a528038)
+	REG_LD_AND_STR_OP(59, 0x01c, 0x03c68039)
+	REG_LD_AND_STR_OP(60, 0x01c, 0x00448039)
+	REG_LD_AND_STR_OP(61, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(62, 0x058, 0x00033335)
+	REG_LD_AND_STR_OP(63, 0x01c, 0x00000000)
+	REG_LD_AND_STR_OP(64, 0x040, 0x04b80003)
+	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
 #endif
+
+	/* For TO2 only, increase LD0 for VDIG_PLL to 1.3V */
+	REG_LD_AND_STR_INIT(IOMUXC_BASE_ADDR)
+	REG_LD_AND_STR_OP(52, 0x4, 0x00194005)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+/*
+ *  The following is to fill in those arguments for this ROM function
+ * pu_irom_hwcnfg_setup(void **start, size_t *bytes, const void *boot_data)
+ *
+ *  This function is used to copy data from the storage media into DDR.
+ *
+ *  start - Initial (possibly partial) image load address on entry. Final image
+ * load address on exit.
+ *  bytes - Initial (possibly partial) image size on entry. Final image size on
+ * exit.
+ *  boot_data - Initial @ref ivt Boot Data load address.
+ */
+	adr r0, DDR_DEST_ADDR
+	adr r1, COPY_SIZE
+	adr r2, BOOT_DATA
+before_calling_rom___pu_irom_hwcnfg_setup:
+
+	/* We should distinguish USB recovery mode(SDP mode) and internal boot mode.
+	If ROM runs in SDP mode, then it needn't load boot code from storage media.
+	If ROM runs in SDP mode, then r0 must be 0x00
+	If ROM runs in internal boot mode, then r0 should be larger than IRAM base address. */
+	cmp r7, #0xF8000000
+	bls return_sdp
+	/* Different ROM address for TO 1.0 & TO 2.0 */
+	ldr r3, =ROM_SI_REV
+	ldr r4, [r3]
+
+	cmp r4, #0x21
+	/* TO2.1 */
+	moveq r6, #0x1800
+	addeq r6, r6, #0x4d
+	beq 2f
+
+	cmp r4, #0x20
+	/* TO2 */
+	moveq r6, #0x1800
+	addeq r6, r6, #0x4d
+	beq 2f
+
+	/* TO1 */
+	mov r6, #0x400000
+	add r6, r6, #0x5000
+	add r6, r6, #0xc7
+
+2:
+	blx r6 /* This address might change in future ROM versions */
+after_calling_rom___pu_irom_hwcnfg_setup:
+
+return_sdp:
+	cmp r7, #0xF8000000
+	bhi quit_plugin
+
+	/* Workaround run plug-ins in SDP mode without USB re-enumeration.
+	how it works:
+	ROM running in usb download mode.
+	Host manufacturing application sends SDP command to download plug-in image.
+	Host manufacturing application sends SDP command to jump to plug-in image and run it.
+	Plug-in starts execution and after its regular tasks plug-in will then call into ROM
+	call into pl_parse_and_handle() */
+	ldr r3, =ROM_SI_REV
+	ldr r5, [r3]
+	cmp r5, #0x20       /* check silicon revision to determine the function entry address */
+
+	ldrlt r4, =0x00000edd   /* function entry in TO1 ROM */
+	ldrge r4, =0x0040487d   /* function entry in TO2 ROM */
+	blx r4
+
+/* To return to ROM from plugin, we need to fill in these argument.
+ * Here is what need to do:
+ * Need to construct the paramters for this function before return to ROM:
+ * plugin_download(void **start, size_t *bytes, UINT32 *ivt_offset)
+ */
+quit_plugin:
+	pop {r0-r6, lr}
+	ldr r4, DDR_DEST_ADDR
+	str r4, [r0]
+	ldr r4, COPY_SIZE
+	str r4, [r1]
+	mov r4, #0x400  /* Point to the second IVT table at offset 0x42C */
+	add r4, r4, #0x2C
+	str r4, [r2]
+	mov r0, #1
+
+	bx lr  /* return back to ROM code */
+
+DDR_DEST_ADDR:    .word   TEXT_BASE
+COPY_SIZE:        .word   _end - TEXT_BASE
+BOOT_DATA:        .word   TEXT_BASE
+                  .word   _end - TEXT_BASE
+                  .word   0
+
 #endif
diff --git a/board/freescale/mx53_evk/flash_header.S b/board/freescale/mx53_evk/flash_header.S
index fcbf111..5a8c2e2 100644
--- a/board/freescale/mx53_evk/flash_header.S
+++ b/board/freescale/mx53_evk/flash_header.S
@@ -20,167 +20,227 @@
 #include <config.h>
 #include <asm/arch/mx53.h>
 
+#define REG_LD_AND_STR_INIT(base)	\
+	ldr r0, =base;
+
+#define REG_LD_AND_STR_OP(i, offset, val)	\
+	ldr r1, =val;	\
+	ldr r2, =offset;	\
+	str r1, [r0, r2];
+
+#define REG_LD_AND_STR_END(base)
+
+
 #ifdef	CONFIG_FLASH_HEADER
 #ifndef CONFIG_FLASH_HEADER_OFFSET
 # error "Must define the offset of flash header"
 #endif
 
-#define CPU_2_BE_32(l) \
-       ((((l) & 0x000000FF) << 24) | \
-	(((l) & 0x0000FF00) << 8)  | \
-	(((l) & 0x00FF0000) >> 8)  | \
-	(((l) & 0xFF000000) >> 24))
-
-#define MXC_DCD_ITEM(i, addr, val)   \
-dcd_node_##i:                        \
-        .word CPU_2_BE_32(addr) ;     \
-        .word CPU_2_BE_32(val)  ;     \
-
 .section ".text.flasheader", "x"
 	b	_start
 	.org	CONFIG_FLASH_HEADER_OFFSET
-ivt_header:       .word 0x402000D1 /* Tag=0xD1, Len=0x0020, Ver=0x40 */
-app_code_jump_v:  .word _start
-reserv1:          .word 0x0
-dcd_ptr:          .word dcd_hdr
-boot_data_ptr:	  .word boot_data
-self_ptr:         .word ivt_header
-app_code_csf:     .word 0x0
-reserv2:          .word 0x0
-
-boot_data:        .word 0x77800000
-image_len:        .word _end_of_copy - TEXT_BASE + CONFIG_FLASH_HEADER_OFFSET
-plugin:           .word 0x0
-
-/* TO1.0/TO2.0 DDR2 scripts for EVK, ARD and ARM2 CPU2 board */
-#if !defined(CONFIG_MX53_ARM2_DDR3)
-dcd_hdr:          .word 0x400802D2 /* Tag=0xD2, Len=64*8 + 4 + 4, Ver=0x40 */
-write_dcd_cmd:    .word 0x040402CC /* Tag=0xCC, Len=64*8 + 4, Param=4 */
-
-/* DCD */
-MXC_DCD_ITEM(1, IOMUXC_BASE_ADDR + 0x554, 0x00200000)
-MXC_DCD_ITEM(2, IOMUXC_BASE_ADDR + 0x560, 0x00200000)
-MXC_DCD_ITEM(3, IOMUXC_BASE_ADDR + 0x594, 0x00200000)
-MXC_DCD_ITEM(4, IOMUXC_BASE_ADDR + 0x584, 0x00200000)
-MXC_DCD_ITEM(5, IOMUXC_BASE_ADDR + 0x558, 0x00200040)
-MXC_DCD_ITEM(6, IOMUXC_BASE_ADDR + 0x568, 0x00200040)
-MXC_DCD_ITEM(7, IOMUXC_BASE_ADDR + 0x590, 0x00200040)
-MXC_DCD_ITEM(8, IOMUXC_BASE_ADDR + 0x57c, 0x00200040)
-MXC_DCD_ITEM(9, IOMUXC_BASE_ADDR + 0x564, 0x00200040)
-MXC_DCD_ITEM(10, IOMUXC_BASE_ADDR + 0x580, 0x00200040)
-MXC_DCD_ITEM(11, IOMUXC_BASE_ADDR + 0x570, 0x00200000)
-MXC_DCD_ITEM(12, IOMUXC_BASE_ADDR + 0x578, 0x00200000)
-MXC_DCD_ITEM(13, IOMUXC_BASE_ADDR + 0x72c, 0x00200000)
-MXC_DCD_ITEM(14, IOMUXC_BASE_ADDR + 0x728, 0x00200000)
-MXC_DCD_ITEM(15, IOMUXC_BASE_ADDR + 0x71c, 0x00200000)
-MXC_DCD_ITEM(16, IOMUXC_BASE_ADDR + 0x718, 0x00200000)
-MXC_DCD_ITEM(17, IOMUXC_BASE_ADDR + 0x574, 0x00280000)
-MXC_DCD_ITEM(18, IOMUXC_BASE_ADDR + 0x588, 0x00280000)
-MXC_DCD_ITEM(19, IOMUXC_BASE_ADDR + 0x6f0, 0x00280000)
-MXC_DCD_ITEM(20, IOMUXC_BASE_ADDR + 0x720, 0x00280000)
-MXC_DCD_ITEM(21, IOMUXC_BASE_ADDR + 0x6fc, 0x00000000)
-MXC_DCD_ITEM(22, IOMUXC_BASE_ADDR + 0x6f4, 0x00000200)
-MXC_DCD_ITEM(23, IOMUXC_BASE_ADDR + 0x714, 0x00000000)
-MXC_DCD_ITEM(24, IOMUXC_BASE_ADDR + 0x724, 0x06000000)
-MXC_DCD_ITEM(25, ESDCTL_BASE_ADDR + 0x088, 0x34333936)
-MXC_DCD_ITEM(26, ESDCTL_BASE_ADDR + 0x090, 0x49434942)
-MXC_DCD_ITEM(27, ESDCTL_BASE_ADDR + 0x0f8, 0x00000800)
-MXC_DCD_ITEM(28, ESDCTL_BASE_ADDR + 0x07c, 0x01350138)
-MXC_DCD_ITEM(29, ESDCTL_BASE_ADDR + 0x080, 0x01380139)
-MXC_DCD_ITEM(30, ESDCTL_BASE_ADDR + 0x018, 0x00001710)
-MXC_DCD_ITEM(31, ESDCTL_BASE_ADDR + 0x000, 0xc4110000)
-MXC_DCD_ITEM(32, ESDCTL_BASE_ADDR + 0x00c, 0x4d5122d2)
-MXC_DCD_ITEM(33, ESDCTL_BASE_ADDR + 0x010, 0x92d18a22)
-MXC_DCD_ITEM(34, ESDCTL_BASE_ADDR + 0x014, 0x00c70092)
-MXC_DCD_ITEM(35, ESDCTL_BASE_ADDR + 0x02c, 0x000026d2)
-MXC_DCD_ITEM(36, ESDCTL_BASE_ADDR + 0x030, 0x009f000e)
-MXC_DCD_ITEM(37, ESDCTL_BASE_ADDR + 0x008, 0x12272000)
-MXC_DCD_ITEM(38, ESDCTL_BASE_ADDR + 0x004, 0x00030012)
-MXC_DCD_ITEM(39, ESDCTL_BASE_ADDR + 0x01c, 0x04008010)
-MXC_DCD_ITEM(40, ESDCTL_BASE_ADDR + 0x01c, 0x00008032)
-MXC_DCD_ITEM(41, ESDCTL_BASE_ADDR + 0x01c, 0x00008033)
-MXC_DCD_ITEM(42, ESDCTL_BASE_ADDR + 0x01c, 0x00008031)
-MXC_DCD_ITEM(43, ESDCTL_BASE_ADDR + 0x01c, 0x0b5280b0)
-MXC_DCD_ITEM(44, ESDCTL_BASE_ADDR + 0x01c, 0x04008010)
-MXC_DCD_ITEM(45, ESDCTL_BASE_ADDR + 0x01c, 0x00008020)
-MXC_DCD_ITEM(46, ESDCTL_BASE_ADDR + 0x01c, 0x00008020)
-MXC_DCD_ITEM(47, ESDCTL_BASE_ADDR + 0x01c, 0x0a528030)
-MXC_DCD_ITEM(48, ESDCTL_BASE_ADDR + 0x01c, 0x03c68031)
-MXC_DCD_ITEM(49, ESDCTL_BASE_ADDR + 0x01c, 0x00448031)
-MXC_DCD_ITEM(50, ESDCTL_BASE_ADDR + 0x01c, 0x04008018)
-MXC_DCD_ITEM(51, ESDCTL_BASE_ADDR + 0x01c, 0x0000803a)
-MXC_DCD_ITEM(52, ESDCTL_BASE_ADDR + 0x01c, 0x0000803b)
-MXC_DCD_ITEM(53, ESDCTL_BASE_ADDR + 0x01c, 0x00008039)
-MXC_DCD_ITEM(54, ESDCTL_BASE_ADDR + 0x01c, 0x0b528138)
-MXC_DCD_ITEM(55, ESDCTL_BASE_ADDR + 0x01c, 0x04008018)
-MXC_DCD_ITEM(56, ESDCTL_BASE_ADDR + 0x01c, 0x00008028)
-MXC_DCD_ITEM(57, ESDCTL_BASE_ADDR + 0x01c, 0x00008028)
-MXC_DCD_ITEM(58, ESDCTL_BASE_ADDR + 0x01c, 0x0a528038)
-MXC_DCD_ITEM(59, ESDCTL_BASE_ADDR + 0x01c, 0x03c68039)
-MXC_DCD_ITEM(60, ESDCTL_BASE_ADDR + 0x01c, 0x00448039)
-MXC_DCD_ITEM(61, ESDCTL_BASE_ADDR + 0x020, 0x00005800)
-MXC_DCD_ITEM(62, ESDCTL_BASE_ADDR + 0x058, 0x00033335)
-MXC_DCD_ITEM(63, ESDCTL_BASE_ADDR + 0x01c, 0x00000000)
-MXC_DCD_ITEM(64, ESDCTL_BASE_ADDR + 0x040, 0x04b80003)
-
-
-#elif defined(CONFIG_MX53_ARM2_DDR3)
-dcd_hdr:          .word 0x40A801D2 /* Tag=0xD2, Len=52*8 + 4 + 4, Ver=0x40 */
-write_dcd_cmd:    .word 0x04A401CC /* Tag=0xCC, Len=52*8 + 4, Param=4 */
-
-/* DCD */
-MXC_DCD_ITEM(1, IOMUXC_BASE_ADDR + 0x554, 0x00300000)
-MXC_DCD_ITEM(2, IOMUXC_BASE_ADDR + 0x560, 0x00300000)
-MXC_DCD_ITEM(3, IOMUXC_BASE_ADDR + 0x594, 0x00300000)
-MXC_DCD_ITEM(4, IOMUXC_BASE_ADDR + 0x584, 0x00300000)
-MXC_DCD_ITEM(5, IOMUXC_BASE_ADDR + 0x558, 0x00300040)
-MXC_DCD_ITEM(6, IOMUXC_BASE_ADDR + 0x568, 0x00300040)
-MXC_DCD_ITEM(7, IOMUXC_BASE_ADDR + 0x590, 0x00300040)
-MXC_DCD_ITEM(8, IOMUXC_BASE_ADDR + 0x57c, 0x00300040)
-MXC_DCD_ITEM(9, IOMUXC_BASE_ADDR + 0x564, 0x00300040)
-MXC_DCD_ITEM(10, IOMUXC_BASE_ADDR + 0x580, 0x00300040)
-MXC_DCD_ITEM(11, IOMUXC_BASE_ADDR + 0x570, 0x00300000)
-MXC_DCD_ITEM(12, IOMUXC_BASE_ADDR + 0x578, 0x00300000)
-MXC_DCD_ITEM(13, IOMUXC_BASE_ADDR + 0x72c, 0x00300000)
-MXC_DCD_ITEM(14, IOMUXC_BASE_ADDR + 0x728, 0x00300000)
-MXC_DCD_ITEM(15, IOMUXC_BASE_ADDR + 0x71c, 0x00300000)
-MXC_DCD_ITEM(16, IOMUXC_BASE_ADDR + 0x718, 0x00300000)
-MXC_DCD_ITEM(17, IOMUXC_BASE_ADDR + 0x574, 0x00300000)
-MXC_DCD_ITEM(18, IOMUXC_BASE_ADDR + 0x588, 0x00300000)
-MXC_DCD_ITEM(19, IOMUXC_BASE_ADDR + 0x6f0, 0x00300000)
-MXC_DCD_ITEM(20, IOMUXC_BASE_ADDR + 0x720, 0x00300000)
-MXC_DCD_ITEM(21, IOMUXC_BASE_ADDR + 0x6fc, 0x00000000)
-MXC_DCD_ITEM(22, IOMUXC_BASE_ADDR + 0x6f4, 0x00000200)
-MXC_DCD_ITEM(23, IOMUXC_BASE_ADDR + 0x714, 0x00000000)
-MXC_DCD_ITEM(24, IOMUXC_BASE_ADDR + 0x724, 0x00000000)
-MXC_DCD_ITEM(25, ESDCTL_BASE_ADDR + 0x098, 0x00000a00)
-MXC_DCD_ITEM(26, ESDCTL_BASE_ADDR + 0x088, 0x32383535)
-MXC_DCD_ITEM(27, ESDCTL_BASE_ADDR + 0x090, 0x40383538)
-MXC_DCD_ITEM(28, ESDCTL_BASE_ADDR + 0x07c, 0x0136014d)
-MXC_DCD_ITEM(29, ESDCTL_BASE_ADDR + 0x080, 0x01510141)
-MXC_DCD_ITEM(30, ESDCTL_BASE_ADDR + 0x018, 0x00091740)
-MXC_DCD_ITEM(31, ESDCTL_BASE_ADDR + 0x000, 0xc4190000)
-MXC_DCD_ITEM(32, ESDCTL_BASE_ADDR + 0x00C, 0x565a7543)
-MXC_DCD_ITEM(33, ESDCTL_BASE_ADDR + 0x010, 0xb6ae8aa3)
-MXC_DCD_ITEM(34, ESDCTL_BASE_ADDR + 0x014, 0x01ff00db)
-MXC_DCD_ITEM(35, ESDCTL_BASE_ADDR + 0x02c, 0x000026d2)
-MXC_DCD_ITEM(36, ESDCTL_BASE_ADDR + 0x030, 0x009f0e21)
-MXC_DCD_ITEM(37, ESDCTL_BASE_ADDR + 0x008, 0x12272000)
-MXC_DCD_ITEM(38, ESDCTL_BASE_ADDR + 0x004, 0x00030012)
-MXC_DCD_ITEM(39, ESDCTL_BASE_ADDR + 0x01c, 0x02008032)
-MXC_DCD_ITEM(40, ESDCTL_BASE_ADDR + 0x01c, 0x00008033)
-MXC_DCD_ITEM(41, ESDCTL_BASE_ADDR + 0x01c, 0x00428031)
-MXC_DCD_ITEM(42, ESDCTL_BASE_ADDR + 0x01c, 0x092080b0)
-MXC_DCD_ITEM(43, ESDCTL_BASE_ADDR + 0x01c, 0x04008040)
-MXC_DCD_ITEM(44, ESDCTL_BASE_ADDR + 0x01c, 0x0200803a)
-MXC_DCD_ITEM(45, ESDCTL_BASE_ADDR + 0x01c, 0x0000803b)
-MXC_DCD_ITEM(46, ESDCTL_BASE_ADDR + 0x01c, 0x00428039)
-MXC_DCD_ITEM(47, ESDCTL_BASE_ADDR + 0x01c, 0x09208138)
-MXC_DCD_ITEM(48, ESDCTL_BASE_ADDR + 0x01c, 0x04008048)
-MXC_DCD_ITEM(49, ESDCTL_BASE_ADDR + 0x020, 0x00001800)
-MXC_DCD_ITEM(50, ESDCTL_BASE_ADDR + 0x040, 0x04b80003)
-MXC_DCD_ITEM(51, ESDCTL_BASE_ADDR + 0x058, 0x00002225)
-MXC_DCD_ITEM(52, ESDCTL_BASE_ADDR + 0x01c, 0x00000000)
 
-#endif
+ivt_header:		.long 0x402000D1/* Tag=0xD1, Len=0x0020, Ver=0x40 */
+app_code_jump_v:	.long (0xF8006000 + (plugin_start - TEXT_BASE))
+reserv1:		.long 0x0
+dcd_ptr:		.long 0x0
+boot_data_ptr:		.long (0xF8006000 + (boot_data - TEXT_BASE))
+self_ptr:		.long (0xF8006000 + (ivt_header - TEXT_BASE))
+app_code_csf:		.long 0x0
+reserv2:		.long 0x0
+boot_data:		.long 0xF8006000
+image_len:		.long (3 * 1024)
+plugin:			.long 0x1
+
+/* Second IVT to give entry point into the bootloader copied to DDR */
+ivt2_header:		.long 0x402000D1/*Tag=0xD1, Len=0x0020, Ver=0x40 */
+app2_code_jump_v:	.long _start   /* Entry point for the bootloader */
+reserv3:		.long 0x0
+dcd2_ptr:		.long 0x0
+boot_data2_ptr:		.long boot_data2
+self_ptr2:		.long ivt2_header
+app_code_csf2:		.long 0x0
+reserv4:		.long 0x0
+boot_data2:		.long TEXT_BASE
+image_len2:		.long  _end_of_copy - TEXT_BASE + CONFIG_FLASH_HEADER_OFFSET
+plugin2:		.long 0x0
+
+/* Here starts the plugin code */
+plugin_start:
+	/* Save the return address and the function arguments */
+	push {r0-r6, lr}
+	/* We should distinguish USB recovery mode(SDP mode) and internal boot mode.
+	If ROM runs in SDP mode, then it needn't load boot code from storage media.
+	If ROM runs in SDP mode, then r0 must be 0x00
+	If ROM runs in internal boot mode, then r0 should be larger than IRAM base address. */
+	mov r7, r0
+
+	/* IOMUX */
+	REG_LD_AND_STR_INIT(IOMUXC_BASE_ADDR)
+	REG_LD_AND_STR_OP(1, 0x554, 0x00200000)
+	REG_LD_AND_STR_OP(2, 0x560, 0x00200000)
+	REG_LD_AND_STR_OP(3, 0x594, 0x00200000)
+	REG_LD_AND_STR_OP(4, 0x584, 0x00200000)
+	REG_LD_AND_STR_OP(5, 0x558, 0x00200040)
+	REG_LD_AND_STR_OP(6, 0x568, 0x00200040)
+	REG_LD_AND_STR_OP(7, 0x590, 0x00200040)
+	REG_LD_AND_STR_OP(8, 0x57c, 0x00200040)
+	REG_LD_AND_STR_OP(9, 0x564, 0x00200040)
+	REG_LD_AND_STR_OP(10, 0x580, 0x00200040)
+	REG_LD_AND_STR_OP(11, 0x570, 0x00200000)
+	REG_LD_AND_STR_OP(12, 0x578, 0x00200000)
+	REG_LD_AND_STR_OP(13, 0x72c, 0x00200000)
+	REG_LD_AND_STR_OP(14, 0x728, 0x00200000)
+	REG_LD_AND_STR_OP(15, 0x71c, 0x00200000)
+	REG_LD_AND_STR_OP(16, 0x718, 0x00200000)
+	REG_LD_AND_STR_OP(17, 0x574, 0x00280000)
+	REG_LD_AND_STR_OP(18, 0x588, 0x00280000)
+	REG_LD_AND_STR_OP(19, 0x6f0, 0x00280000)
+	REG_LD_AND_STR_OP(20, 0x720, 0x00280000)
+	REG_LD_AND_STR_OP(21, 0x6fc, 0x00000000)
+	REG_LD_AND_STR_OP(22, 0x6f4, 0x00000200)
+	REG_LD_AND_STR_OP(23, 0x714, 0x00000000)
+	REG_LD_AND_STR_OP(24, 0x724, 0x06000000)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+	/* ESDCTL */
+	REG_LD_AND_STR_INIT(ESDCTL_BASE_ADDR)
+	REG_LD_AND_STR_OP(25, 0x088, 0x34333936)
+	REG_LD_AND_STR_OP(26, 0x090, 0x49434942)
+	REG_LD_AND_STR_OP(27, 0x0f8, 0x00000800)
+	REG_LD_AND_STR_OP(28, 0x07c, 0x01350138)
+	REG_LD_AND_STR_OP(29, 0x080, 0x01380139)
+	REG_LD_AND_STR_OP(30, 0x018, 0x00001710)
+	REG_LD_AND_STR_OP(31, 0x000, 0xc4110000)
+	REG_LD_AND_STR_OP(32, 0x00c, 0x4d5122d2)
+	REG_LD_AND_STR_OP(33, 0x010, 0x92d18a22)
+	REG_LD_AND_STR_OP(34, 0x014, 0x00c70092)
+	REG_LD_AND_STR_OP(35, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(36, 0x030, 0x009f000e)
+	REG_LD_AND_STR_OP(37, 0x008, 0x12272000)
+	REG_LD_AND_STR_OP(38, 0x004, 0x00030012)
+	REG_LD_AND_STR_OP(39, 0x01c, 0x04008010)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x00008031)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x0b5280b0)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x04008010)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x00008020)
+	REG_LD_AND_STR_OP(46, 0x01c, 0x00008020)
+	REG_LD_AND_STR_OP(47, 0x01c, 0x0a528030)
+	REG_LD_AND_STR_OP(48, 0x01c, 0x03c68031)
+	REG_LD_AND_STR_OP(49, 0x01c, 0x00448031)
+	REG_LD_AND_STR_OP(50, 0x01c, 0x04008018)
+	REG_LD_AND_STR_OP(51, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(52, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(53, 0x01c, 0x00008039)
+	REG_LD_AND_STR_OP(54, 0x01c, 0x0b528138)
+	REG_LD_AND_STR_OP(55, 0x01c, 0x04008018)
+	REG_LD_AND_STR_OP(56, 0x01c, 0x00008028)
+	REG_LD_AND_STR_OP(57, 0x01c, 0x00008028)
+	REG_LD_AND_STR_OP(58, 0x01c, 0x0a528038)
+	REG_LD_AND_STR_OP(59, 0x01c, 0x03c68039)
+	REG_LD_AND_STR_OP(60, 0x01c, 0x00448039)
+	REG_LD_AND_STR_OP(61, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(62, 0x058, 0x00033335)
+	REG_LD_AND_STR_OP(63, 0x01c, 0x00000000)
+	REG_LD_AND_STR_OP(64, 0x040, 0x04b80003)
+	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
+
+	/* For TO2 only, increase LD0 for VDIG_PLL to 1.3V */
+	REG_LD_AND_STR_INIT(IOMUXC_BASE_ADDR)
+	REG_LD_AND_STR_OP(52, 0x4, 0x00194005)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+/*
+ *  The following is to fill in those arguments for this ROM function
+ * pu_irom_hwcnfg_setup(void **start, size_t *bytes, const void *boot_data)
+ *
+ *  This function is used to copy data from the storage media into DDR.
+ *
+ *  start - Initial (possibly partial) image load address on entry. Final image
+ * load address on exit.
+ *  bytes - Initial (possibly partial) image size on entry. Final image size on
+ * exit.
+ *  boot_data - Initial @ref ivt Boot Data load address.
+ */
+	adr r0, DDR_DEST_ADDR
+	adr r1, COPY_SIZE
+	adr r2, BOOT_DATA
+before_calling_rom___pu_irom_hwcnfg_setup:
+
+	/* We should distinguish USB recovery mode(SDP mode) and internal boot mode.
+	If ROM runs in SDP mode, then it needn't load boot code from storage media.
+	If ROM runs in SDP mode, then r0 must be 0x00
+	If ROM runs in internal boot mode, then r0 should be larger than IRAM base address. */
+	cmp r7, #0xF8000000
+	bls return_sdp
+	/* Different ROM address for TO 1.0 & TO 2.0 */
+	ldr r3, =ROM_SI_REV
+	ldr r4, [r3]
+
+	cmp r4, #0x21
+	/* TO2.1 */
+	moveq r6, #0x1800
+	addeq r6, r6, #0x4d
+	beq 2f
+
+	cmp r4, #0x20
+	/* TO2 */
+	moveq r6, #0x1800
+	addeq r6, r6, #0x4d
+	beq 2f
+
+	/* TO1 */
+	mov r6, #0x400000
+	add r6, r6, #0x5000
+	add r6, r6, #0xc7
+
+2:
+	blx r6 /* This address might change in future ROM versions */
+after_calling_rom___pu_irom_hwcnfg_setup:
+
+return_sdp:
+	cmp r7, #0xF8000000
+	bhi quit_plugin
+
+	/* Workaround run plug-ins in SDP mode without USB re-enumeration.
+	how it works:
+	ROM running in usb download mode.
+	Host manufacturing application sends SDP command to download plug-in image.
+	Host manufacturing application sends SDP command to jump to plug-in image and run it.
+	Plug-in starts execution and after its regular tasks plug-in will then call into ROM
+	call into pl_parse_and_handle() */
+	ldr r3, =ROM_SI_REV
+	ldr r5, [r3]
+	cmp r5, #0x20       /* check silicon revision to determine the function entry address */
+
+	ldrlt r4, =0x00000edd   /* function entry in TO1 ROM */
+	ldrge r4, =0x0040487d   /* function entry in TO2 ROM */
+	blx r4
+
+/* To return to ROM from plugin, we need to fill in these argument.
+ * Here is what need to do:
+ * Need to construct the paramters for this function before return to ROM:
+ * plugin_download(void **start, size_t *bytes, UINT32 *ivt_offset)
+ */
+quit_plugin:
+	pop {r0-r6, lr}
+	ldr r4, DDR_DEST_ADDR
+	str r4, [r0]
+	ldr r4, COPY_SIZE
+	str r4, [r1]
+	mov r4, #0x400  /* Point to the second IVT table at offset 0x42C */
+	add r4, r4, #0x2C
+	str r4, [r2]
+	mov r0, #1
+
+	bx lr  /* return back to ROM code */
+
+DDR_DEST_ADDR:    .word   TEXT_BASE
+COPY_SIZE:        .word   _end - TEXT_BASE
+BOOT_DATA:        .word   TEXT_BASE
+                  .word   _end - TEXT_BASE
+                  .word   0
+
 #endif
diff --git a/board/freescale/mx53_loco/flash_header.S b/board/freescale/mx53_loco/flash_header.S
index 03d5400..16adb99 100644
--- a/board/freescale/mx53_loco/flash_header.S
+++ b/board/freescale/mx53_loco/flash_header.S
@@ -20,92 +20,214 @@
 #include <config.h>
 #include <asm/arch/mx53.h>
 
+#define REG_LD_AND_STR_INIT(base)	\
+	ldr r0, =base;
+
+#define REG_LD_AND_STR_OP(i, offset, val)	\
+	ldr r1, =val;	\
+	ldr r2, =offset;	\
+	str r1, [r0, r2];
+
+#define REG_LD_AND_STR_END(base)
+
+
 #ifdef	CONFIG_FLASH_HEADER
 #ifndef CONFIG_FLASH_HEADER_OFFSET
 # error "Must define the offset of flash header"
 #endif
 
-#define CPU_2_BE_32(l) \
-       ((((l) & 0x000000FF) << 24) | \
-	(((l) & 0x0000FF00) << 8)  | \
-	(((l) & 0x00FF0000) >> 8)  | \
-	(((l) & 0xFF000000) >> 24))
-
-#define MXC_DCD_ITEM(i, addr, val)   \
-dcd_node_##i:                        \
-        .word CPU_2_BE_32(addr) ;     \
-        .word CPU_2_BE_32(val)  ;     \
-
 .section ".text.flasheader", "x"
 	b	_start
 	.org	CONFIG_FLASH_HEADER_OFFSET
-ivt_header:       .word 0x402000D1 /* Tag=0xD1, Len=0x0020, Ver=0x40 */
-app_code_jump_v:  .word _start
-reserv1:          .word 0x0
-dcd_ptr:          .word dcd_hdr
-boot_data_ptr:	  .word boot_data
-self_ptr:         .word ivt_header
-app_code_csf:     .word 0x0
-reserv2:          .word 0x0
-
-boot_data:        .word 0x77800000
-image_len:        .word _end_of_copy - TEXT_BASE + CONFIG_FLASH_HEADER_OFFSET
-plugin:           .word 0x0
-
-dcd_hdr:          .word 0x40A001D2 /* Tag=0xD2, Len=51*8 + 4 + 4, Ver=0x40 */
-write_dcd_cmd:    .word 0x049C01CC /* Tag=0xCC, Len=51*8 + 4, Param=4 */
-
-/* DCD */
-MXC_DCD_ITEM(1, IOMUXC_BASE_ADDR + 0x554, 0x00300000)
-MXC_DCD_ITEM(2, IOMUXC_BASE_ADDR + 0x558, 0x00300040)
-MXC_DCD_ITEM(3, IOMUXC_BASE_ADDR + 0x560, 0x00300000)
-MXC_DCD_ITEM(4, IOMUXC_BASE_ADDR + 0x564, 0x00300040)
-MXC_DCD_ITEM(5, IOMUXC_BASE_ADDR + 0x568, 0x00300040)
-MXC_DCD_ITEM(6, IOMUXC_BASE_ADDR + 0x570, 0x00300000)
-MXC_DCD_ITEM(7, IOMUXC_BASE_ADDR + 0x574, 0x00300000)
-MXC_DCD_ITEM(8, IOMUXC_BASE_ADDR + 0x578, 0x00300000)
-MXC_DCD_ITEM(9, IOMUXC_BASE_ADDR + 0x57c, 0x00300040)
-MXC_DCD_ITEM(10, IOMUXC_BASE_ADDR + 0x580, 0x00300040)
-MXC_DCD_ITEM(11, IOMUXC_BASE_ADDR + 0x584, 0x00300000)
-MXC_DCD_ITEM(12, IOMUXC_BASE_ADDR + 0x588, 0x00300000)
-MXC_DCD_ITEM(13, IOMUXC_BASE_ADDR + 0x590, 0x00300040)
-MXC_DCD_ITEM(14, IOMUXC_BASE_ADDR + 0x594, 0x00300000)
-MXC_DCD_ITEM(15, IOMUXC_BASE_ADDR + 0x6f0, 0x00300000)
-MXC_DCD_ITEM(16, IOMUXC_BASE_ADDR + 0x6f4, 0x00000000)
-MXC_DCD_ITEM(17, IOMUXC_BASE_ADDR + 0x6fc, 0x00000000)
-MXC_DCD_ITEM(18, IOMUXC_BASE_ADDR + 0x714, 0x00000000)
-MXC_DCD_ITEM(19, IOMUXC_BASE_ADDR + 0x718, 0x00300000)
-MXC_DCD_ITEM(20, IOMUXC_BASE_ADDR + 0x71c, 0x00300000)
-MXC_DCD_ITEM(21, IOMUXC_BASE_ADDR + 0x720, 0x00300000)
-MXC_DCD_ITEM(22, IOMUXC_BASE_ADDR + 0x724, 0x04000000)
-MXC_DCD_ITEM(23, IOMUXC_BASE_ADDR + 0x728, 0x00300000)
-MXC_DCD_ITEM(24, IOMUXC_BASE_ADDR + 0x72c, 0x00300000)
-MXC_DCD_ITEM(25, ESDCTL_BASE_ADDR + 0x088, 0x35343535)
-MXC_DCD_ITEM(26, ESDCTL_BASE_ADDR + 0x090, 0x4d444c44)
-MXC_DCD_ITEM(27, ESDCTL_BASE_ADDR + 0x07c, 0x01370138)
-MXC_DCD_ITEM(28, ESDCTL_BASE_ADDR + 0x080, 0x013b013c)
-MXC_DCD_ITEM(29, ESDCTL_BASE_ADDR + 0x018, 0x00011740)
-MXC_DCD_ITEM(30, ESDCTL_BASE_ADDR + 0x000, 0xc3190000)
-MXC_DCD_ITEM(31, ESDCTL_BASE_ADDR + 0x00c, 0x9f5152e3)
-MXC_DCD_ITEM(32, ESDCTL_BASE_ADDR + 0x010, 0xb68e8a63)
-MXC_DCD_ITEM(33, ESDCTL_BASE_ADDR + 0x014, 0x01ff00db)
-MXC_DCD_ITEM(34, ESDCTL_BASE_ADDR + 0x02c, 0x000026d2)
-MXC_DCD_ITEM(35, ESDCTL_BASE_ADDR + 0x030, 0x009f0e21)
-MXC_DCD_ITEM(36, ESDCTL_BASE_ADDR + 0x008, 0x12273030)
-MXC_DCD_ITEM(37, ESDCTL_BASE_ADDR + 0x004, 0x0002002d)
-MXC_DCD_ITEM(38, ESDCTL_BASE_ADDR + 0x01c, 0x00008032)
-MXC_DCD_ITEM(39, ESDCTL_BASE_ADDR + 0x01c, 0x00008033)
-MXC_DCD_ITEM(40, ESDCTL_BASE_ADDR + 0x01c, 0x00028031)
-MXC_DCD_ITEM(41, ESDCTL_BASE_ADDR + 0x01c, 0x052080b0)
-MXC_DCD_ITEM(42, ESDCTL_BASE_ADDR + 0x01c, 0x04008040)
-MXC_DCD_ITEM(43, ESDCTL_BASE_ADDR + 0x01c, 0x0000803a)
-MXC_DCD_ITEM(44, ESDCTL_BASE_ADDR + 0x01c, 0x0000803b)
-MXC_DCD_ITEM(45, ESDCTL_BASE_ADDR + 0x01c, 0x00028039)
-MXC_DCD_ITEM(46, ESDCTL_BASE_ADDR + 0x01c, 0x05208138)
-MXC_DCD_ITEM(47, ESDCTL_BASE_ADDR + 0x01c, 0x04008048)
-MXC_DCD_ITEM(48, ESDCTL_BASE_ADDR + 0x020, 0x00005800)
-MXC_DCD_ITEM(49, ESDCTL_BASE_ADDR + 0x040, 0x04b80003)
-MXC_DCD_ITEM(50, ESDCTL_BASE_ADDR + 0x058, 0x00022227)
-MXC_DCD_ITEM(51, ESDCTL_BASE_ADDR + 0x01C, 0x00000000)
+
+ivt_header:		.long 0x402000D1/* Tag=0xD1, Len=0x0020, Ver=0x40 */
+app_code_jump_v:	.long (0xF8006000 + (plugin_start - TEXT_BASE))
+reserv1:		.long 0x0
+dcd_ptr:		.long 0x0
+boot_data_ptr:		.long (0xF8006000 + (boot_data - TEXT_BASE))
+self_ptr:		.long (0xF8006000 + (ivt_header - TEXT_BASE))
+app_code_csf:		.long 0x0
+reserv2:		.long 0x0
+boot_data:		.long 0xF8006000
+image_len:		.long (3 * 1024)
+plugin:			.long 0x1
+
+/* Second IVT to give entry point into the bootloader copied to DDR */
+ivt2_header:		.long 0x402000D1/*Tag=0xD1, Len=0x0020, Ver=0x40 */
+app2_code_jump_v:	.long _start   /* Entry point for the bootloader */
+reserv3:		.long 0x0
+dcd2_ptr:		.long 0x0
+boot_data2_ptr:		.long boot_data2
+self_ptr2:		.long ivt2_header
+app_code_csf2:		.long 0x0
+reserv4:		.long 0x0
+boot_data2:		.long TEXT_BASE
+image_len2:		.long  _end_of_copy - TEXT_BASE + CONFIG_FLASH_HEADER_OFFSET
+plugin2:		.long 0x0
+
+/* Here starts the plugin code */
+plugin_start:
+	/* Save the return address and the function arguments */
+	push {r0-r6, lr}
+	/* We should distinguish USB recovery mode(SDP mode) and internal boot mode.
+	If ROM runs in SDP mode, then it needn't load boot code from storage media.
+	If ROM runs in SDP mode, then r0 must be 0x00
+	If ROM runs in internal boot mode, then r0 should be larger than IRAM base address. */
+	mov r7, r0
+
+	/* IOMUX */
+	REG_LD_AND_STR_INIT(IOMUXC_BASE_ADDR)
+	REG_LD_AND_STR_OP(1, 0x554, 0x00300000)
+	REG_LD_AND_STR_OP(2, 0x558, 0x00300040)
+	REG_LD_AND_STR_OP(3, 0x560, 0x00300000)
+	REG_LD_AND_STR_OP(4, 0x564, 0x00300040)
+	REG_LD_AND_STR_OP(5, 0x568, 0x00300040)
+	REG_LD_AND_STR_OP(6, 0x570, 0x00300000)
+	REG_LD_AND_STR_OP(7, 0x574, 0x00300000)
+	REG_LD_AND_STR_OP(8, 0x578, 0x00300000)
+	REG_LD_AND_STR_OP(9, 0x57c, 0x00300040)
+	REG_LD_AND_STR_OP(10, 0x580, 0x00300040)
+	REG_LD_AND_STR_OP(11, 0x584, 0x00300000)
+	REG_LD_AND_STR_OP(12, 0x588, 0x00300000)
+	REG_LD_AND_STR_OP(13, 0x590, 0x00300040)
+	REG_LD_AND_STR_OP(14, 0x594, 0x00300000)
+	REG_LD_AND_STR_OP(15, 0x6f0, 0x00300000)
+	REG_LD_AND_STR_OP(16, 0x6f4, 0x00000000)
+	REG_LD_AND_STR_OP(17, 0x6fc, 0x00000000)
+	REG_LD_AND_STR_OP(18, 0x714, 0x00000000)
+	REG_LD_AND_STR_OP(19, 0x718, 0x00300000)
+	REG_LD_AND_STR_OP(20, 0x71c, 0x00300000)
+	REG_LD_AND_STR_OP(21, 0x720, 0x00300000)
+	REG_LD_AND_STR_OP(22, 0x724, 0x04000000)
+	REG_LD_AND_STR_OP(23, 0x728, 0x00300000)
+	REG_LD_AND_STR_OP(24, 0x72c, 0x00300000)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+	/* ESDCTL */
+	REG_LD_AND_STR_INIT(ESDCTL_BASE_ADDR)
+	REG_LD_AND_STR_OP(25, 0x088, 0x35343535)
+	REG_LD_AND_STR_OP(26, 0x090, 0x4d444c44)
+	REG_LD_AND_STR_OP(27, 0x07c, 0x01370138)
+	REG_LD_AND_STR_OP(28, 0x080, 0x013b013c)
+	REG_LD_AND_STR_OP(29, 0x018, 0x00001740)
+	REG_LD_AND_STR_OP(30, 0x000, 0xc3190000)
+	REG_LD_AND_STR_OP(31, 0x00c, 0x9f5152e3)
+	REG_LD_AND_STR_OP(32, 0x010, 0xb68e8a63)
+	REG_LD_AND_STR_OP(33, 0x014, 0x01ff00db)
+	REG_LD_AND_STR_OP(34, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(35, 0x030, 0x009f0e21)
+	REG_LD_AND_STR_OP(36, 0x008, 0x12273030)
+	REG_LD_AND_STR_OP(37, 0x004, 0x0002002d)
+	REG_LD_AND_STR_OP(38, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(39, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x00028031)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x052080b0)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x04008040)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x00028039)
+	REG_LD_AND_STR_OP(46, 0x01c, 0x05208138)
+	REG_LD_AND_STR_OP(47, 0x01c, 0x04008048)
+	REG_LD_AND_STR_OP(48, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(49, 0x040, 0x04b80003)
+	REG_LD_AND_STR_OP(50, 0x058, 0x00022227)
+	REG_LD_AND_STR_OP(51, 0x01C, 0x00000000)
+	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
+
+	/* For TO2 only, increase LD0 for VDIG_PLL to 1.3V */
+	REG_LD_AND_STR_INIT(IOMUXC_BASE_ADDR)
+	REG_LD_AND_STR_OP(52, 0x4, 0x00194005)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+/*
+ *  The following is to fill in those arguments for this ROM function
+ * pu_irom_hwcnfg_setup(void **start, size_t *bytes, const void *boot_data)
+ *
+ *  This function is used to copy data from the storage media into DDR.
+ *
+ *  start - Initial (possibly partial) image load address on entry. Final image
+ *            load address on exit.
+ *  bytes - Initial (possibly partial) image size on entry. Final image size on
+ *             exit.
+ *  boot_data - Initial @ref ivt Boot Data load address.
+ */
+	adr r0, DDR_DEST_ADDR
+	adr r1, COPY_SIZE
+	adr r2, BOOT_DATA
+before_calling_rom___pu_irom_hwcnfg_setup:
+
+	/* We should distinguish USB recovery mode(SDP mode) and internal boot mode.
+	If ROM runs in SDP mode, then it needn't load boot code from storage media.
+	If ROM runs in SDP mode, then r0 must be 0x00
+	If ROM runs in internal boot mode, then r0 should be larger than IRAM base address. */
+	cmp r7, #0xF8000000
+	bls return_sdp
+	/* Different ROM address for TO 1.0 & TO 2.0 */
+	ldr r3, =ROM_SI_REV
+	ldr r4, [r3]
+
+	cmp r4, #0x21
+	/* TO2.1 */
+	moveq r6, #0x1800
+	addeq r6, r6, #0x4d
+	beq 2f
+
+	cmp r4, #0x20
+	/* TO2 */
+	moveq r6, #0x1800
+	addeq r6, r6, #0x4d
+	beq 2f
+
+	/* TO1 */
+	mov r6, #0x400000
+	add r6, r6, #0x5000
+	add r6, r6, #0xc7
+
+2:
+	blx r6 /* This address might change in future ROM versions */
+after_calling_rom___pu_irom_hwcnfg_setup:
+
+return_sdp:
+	cmp r7, #0xF8000000
+	bhi quit_plugin
+
+	/* Workaround run plug-ins in SDP mode without USB re-enumeration.
+	how it works:
+	ROM running in usb download mode.
+	Host manufacturing application sends SDP command to download plug-in image.
+	Host manufacturing application sends SDP command to jump to plug-in image and run it.
+	Plug-in starts execution and after its regular tasks plug-in will then call into ROM
+	call into pl_parse_and_handle() */
+	ldr r3, =ROM_SI_REV
+	ldr r5, [r3]
+	cmp r5, #0x20       /* check silicon revision to determine the function entry address */
+
+	ldrlt r4, =0x00000edd   /* function entry in TO1 ROM */
+	ldrge r4, =0x0040487d   /* function entry in TO2 ROM */
+	blx r4
+
+/* To return to ROM from plugin, we need to fill in these argument.
+ * Here is what need to do:
+ * Need to construct the paramters for this function before return to ROM:
+ * plugin_download(void **start, size_t *bytes, UINT32 *ivt_offset)
+ */
+quit_plugin:
+	pop {r0-r6, lr}
+	ldr r4, DDR_DEST_ADDR
+	str r4, [r0]
+	ldr r4, COPY_SIZE
+	str r4, [r1]
+	mov r4, #0x400  /* Point to the second IVT table at offset 0x42C */
+	add r4, r4, #0x2C
+	str r4, [r2]
+	mov r0, #1
+
+	bx lr  /* return back to ROM code */
+
+DDR_DEST_ADDR:    .word   TEXT_BASE
+COPY_SIZE:        .word   _end - TEXT_BASE
+BOOT_DATA:        .word   TEXT_BASE
+                  .word   _end - TEXT_BASE
+                  .word   0
 
 #endif
diff --git a/board/freescale/mx53_smd/flash_header.S b/board/freescale/mx53_smd/flash_header.S
index 03d5400..1dc22a2 100644
--- a/board/freescale/mx53_smd/flash_header.S
+++ b/board/freescale/mx53_smd/flash_header.S
@@ -20,92 +20,212 @@
 #include <config.h>
 #include <asm/arch/mx53.h>
 
+#define REG_LD_AND_STR_INIT(base)	\
+	ldr r0, =base;
+
+#define REG_LD_AND_STR_OP(i, offset, val)	\
+	ldr r1, =val;	\
+	ldr r2, =offset;	\
+	str r1, [r0, r2];
+
+#define REG_LD_AND_STR_END(base)
+
+
 #ifdef	CONFIG_FLASH_HEADER
 #ifndef CONFIG_FLASH_HEADER_OFFSET
 # error "Must define the offset of flash header"
 #endif
 
-#define CPU_2_BE_32(l) \
-       ((((l) & 0x000000FF) << 24) | \
-	(((l) & 0x0000FF00) << 8)  | \
-	(((l) & 0x00FF0000) >> 8)  | \
-	(((l) & 0xFF000000) >> 24))
-
-#define MXC_DCD_ITEM(i, addr, val)   \
-dcd_node_##i:                        \
-        .word CPU_2_BE_32(addr) ;     \
-        .word CPU_2_BE_32(val)  ;     \
-
 .section ".text.flasheader", "x"
 	b	_start
 	.org	CONFIG_FLASH_HEADER_OFFSET
-ivt_header:       .word 0x402000D1 /* Tag=0xD1, Len=0x0020, Ver=0x40 */
-app_code_jump_v:  .word _start
-reserv1:          .word 0x0
-dcd_ptr:          .word dcd_hdr
-boot_data_ptr:	  .word boot_data
-self_ptr:         .word ivt_header
-app_code_csf:     .word 0x0
-reserv2:          .word 0x0
-
-boot_data:        .word 0x77800000
-image_len:        .word _end_of_copy - TEXT_BASE + CONFIG_FLASH_HEADER_OFFSET
-plugin:           .word 0x0
-
-dcd_hdr:          .word 0x40A001D2 /* Tag=0xD2, Len=51*8 + 4 + 4, Ver=0x40 */
-write_dcd_cmd:    .word 0x049C01CC /* Tag=0xCC, Len=51*8 + 4, Param=4 */
-
-/* DCD */
-MXC_DCD_ITEM(1, IOMUXC_BASE_ADDR + 0x554, 0x00300000)
-MXC_DCD_ITEM(2, IOMUXC_BASE_ADDR + 0x558, 0x00300040)
-MXC_DCD_ITEM(3, IOMUXC_BASE_ADDR + 0x560, 0x00300000)
-MXC_DCD_ITEM(4, IOMUXC_BASE_ADDR + 0x564, 0x00300040)
-MXC_DCD_ITEM(5, IOMUXC_BASE_ADDR + 0x568, 0x00300040)
-MXC_DCD_ITEM(6, IOMUXC_BASE_ADDR + 0x570, 0x00300000)
-MXC_DCD_ITEM(7, IOMUXC_BASE_ADDR + 0x574, 0x00300000)
-MXC_DCD_ITEM(8, IOMUXC_BASE_ADDR + 0x578, 0x00300000)
-MXC_DCD_ITEM(9, IOMUXC_BASE_ADDR + 0x57c, 0x00300040)
-MXC_DCD_ITEM(10, IOMUXC_BASE_ADDR + 0x580, 0x00300040)
-MXC_DCD_ITEM(11, IOMUXC_BASE_ADDR + 0x584, 0x00300000)
-MXC_DCD_ITEM(12, IOMUXC_BASE_ADDR + 0x588, 0x00300000)
-MXC_DCD_ITEM(13, IOMUXC_BASE_ADDR + 0x590, 0x00300040)
-MXC_DCD_ITEM(14, IOMUXC_BASE_ADDR + 0x594, 0x00300000)
-MXC_DCD_ITEM(15, IOMUXC_BASE_ADDR + 0x6f0, 0x00300000)
-MXC_DCD_ITEM(16, IOMUXC_BASE_ADDR + 0x6f4, 0x00000000)
-MXC_DCD_ITEM(17, IOMUXC_BASE_ADDR + 0x6fc, 0x00000000)
-MXC_DCD_ITEM(18, IOMUXC_BASE_ADDR + 0x714, 0x00000000)
-MXC_DCD_ITEM(19, IOMUXC_BASE_ADDR + 0x718, 0x00300000)
-MXC_DCD_ITEM(20, IOMUXC_BASE_ADDR + 0x71c, 0x00300000)
-MXC_DCD_ITEM(21, IOMUXC_BASE_ADDR + 0x720, 0x00300000)
-MXC_DCD_ITEM(22, IOMUXC_BASE_ADDR + 0x724, 0x04000000)
-MXC_DCD_ITEM(23, IOMUXC_BASE_ADDR + 0x728, 0x00300000)
-MXC_DCD_ITEM(24, IOMUXC_BASE_ADDR + 0x72c, 0x00300000)
-MXC_DCD_ITEM(25, ESDCTL_BASE_ADDR + 0x088, 0x35343535)
-MXC_DCD_ITEM(26, ESDCTL_BASE_ADDR + 0x090, 0x4d444c44)
-MXC_DCD_ITEM(27, ESDCTL_BASE_ADDR + 0x07c, 0x01370138)
-MXC_DCD_ITEM(28, ESDCTL_BASE_ADDR + 0x080, 0x013b013c)
-MXC_DCD_ITEM(29, ESDCTL_BASE_ADDR + 0x018, 0x00011740)
-MXC_DCD_ITEM(30, ESDCTL_BASE_ADDR + 0x000, 0xc3190000)
-MXC_DCD_ITEM(31, ESDCTL_BASE_ADDR + 0x00c, 0x9f5152e3)
-MXC_DCD_ITEM(32, ESDCTL_BASE_ADDR + 0x010, 0xb68e8a63)
-MXC_DCD_ITEM(33, ESDCTL_BASE_ADDR + 0x014, 0x01ff00db)
-MXC_DCD_ITEM(34, ESDCTL_BASE_ADDR + 0x02c, 0x000026d2)
-MXC_DCD_ITEM(35, ESDCTL_BASE_ADDR + 0x030, 0x009f0e21)
-MXC_DCD_ITEM(36, ESDCTL_BASE_ADDR + 0x008, 0x12273030)
-MXC_DCD_ITEM(37, ESDCTL_BASE_ADDR + 0x004, 0x0002002d)
-MXC_DCD_ITEM(38, ESDCTL_BASE_ADDR + 0x01c, 0x00008032)
-MXC_DCD_ITEM(39, ESDCTL_BASE_ADDR + 0x01c, 0x00008033)
-MXC_DCD_ITEM(40, ESDCTL_BASE_ADDR + 0x01c, 0x00028031)
-MXC_DCD_ITEM(41, ESDCTL_BASE_ADDR + 0x01c, 0x052080b0)
-MXC_DCD_ITEM(42, ESDCTL_BASE_ADDR + 0x01c, 0x04008040)
-MXC_DCD_ITEM(43, ESDCTL_BASE_ADDR + 0x01c, 0x0000803a)
-MXC_DCD_ITEM(44, ESDCTL_BASE_ADDR + 0x01c, 0x0000803b)
-MXC_DCD_ITEM(45, ESDCTL_BASE_ADDR + 0x01c, 0x00028039)
-MXC_DCD_ITEM(46, ESDCTL_BASE_ADDR + 0x01c, 0x05208138)
-MXC_DCD_ITEM(47, ESDCTL_BASE_ADDR + 0x01c, 0x04008048)
-MXC_DCD_ITEM(48, ESDCTL_BASE_ADDR + 0x020, 0x00005800)
-MXC_DCD_ITEM(49, ESDCTL_BASE_ADDR + 0x040, 0x04b80003)
-MXC_DCD_ITEM(50, ESDCTL_BASE_ADDR + 0x058, 0x00022227)
-MXC_DCD_ITEM(51, ESDCTL_BASE_ADDR + 0x01C, 0x00000000)
+
+ivt_header:		.long 0x402000D1/* Tag=0xD1, Len=0x0020, Ver=0x40 */
+app_code_jump_v:	.long (0xF8006000 + (plugin_start - TEXT_BASE))
+reserv1:		.long 0x0
+dcd_ptr:		.long 0x0
+boot_data_ptr:		.long (0xF8006000 + (boot_data - TEXT_BASE))
+self_ptr:		.long (0xF8006000 + (ivt_header - TEXT_BASE))
+app_code_csf:		.long 0x0
+reserv2:		.long 0x0
+boot_data:		.long 0xF8006000
+image_len:		.long (3 * 1024)
+plugin:			.long 0x1
+
+/* Second IVT to give entry point into the bootloader copied to DDR */
+ivt2_header:		.long 0x402000D1/*Tag=0xD1, Len=0x0020, Ver=0x40 */
+app2_code_jump_v:	.long _start   /* Entry point for the bootloader */
+reserv3:		.long 0x0
+dcd2_ptr:		.long 0x0
+boot_data2_ptr:		.long boot_data2
+self_ptr2:		.long ivt2_header
+app_code_csf2:		.long 0x0
+reserv4:		.long 0x0
+boot_data2:		.long TEXT_BASE
+image_len2:		.long  _end_of_copy - TEXT_BASE + CONFIG_FLASH_HEADER_OFFSET
+plugin2:		.long 0x0
+
+/* Here starts the plugin code */
+plugin_start:
+	/* Save the return address and the function arguments */
+	push {r0-r6, lr}
+	/* We should distinguish USB recovery mode(SDP mode) and internal boot mode.
+	If ROM runs in SDP mode, then it needn't load boot code from storage media.
+	If ROM runs in SDP mode, then r0 must be 0x00
+	If ROM runs in internal boot mode, then r0 should be larger than IRAM base address. */
+	mov r7, r0
+
+	/* IOMUX */
+	REG_LD_AND_STR_INIT(IOMUXC_BASE_ADDR)
+	REG_LD_AND_STR_OP(1, 0x554, 0x00300000)
+	REG_LD_AND_STR_OP(2, 0x558, 0x00300040)
+	REG_LD_AND_STR_OP(3, 0x560, 0x00300000)
+	REG_LD_AND_STR_OP(4, 0x564, 0x00300040)
+	REG_LD_AND_STR_OP(5, 0x568, 0x00300040)
+	REG_LD_AND_STR_OP(6, 0x570, 0x00300000)
+	REG_LD_AND_STR_OP(7, 0x574, 0x00300000)
+	REG_LD_AND_STR_OP(8, 0x578, 0x00300000)
+	REG_LD_AND_STR_OP(9, 0x57c, 0x00300040)
+	REG_LD_AND_STR_OP(10, 0x580, 0x00300040)
+	REG_LD_AND_STR_OP(11, 0x584, 0x00300000)
+	REG_LD_AND_STR_OP(12, 0x588, 0x00300000)
+	REG_LD_AND_STR_OP(13, 0x590, 0x00300040)
+	REG_LD_AND_STR_OP(14, 0x594, 0x00300000)
+	REG_LD_AND_STR_OP(15, 0x6f0, 0x00300000)
+	REG_LD_AND_STR_OP(16, 0x6f4, 0x00000000)
+	REG_LD_AND_STR_OP(17, 0x6fc, 0x00000000)
+	REG_LD_AND_STR_OP(18, 0x714, 0x00000000)
+	REG_LD_AND_STR_OP(19, 0x718, 0x00300000)
+	REG_LD_AND_STR_OP(20, 0x71c, 0x00300000)
+	REG_LD_AND_STR_OP(21, 0x720, 0x00300000)
+	REG_LD_AND_STR_OP(22, 0x724, 0x04000000)
+	REG_LD_AND_STR_OP(23, 0x728, 0x00300000)
+	REG_LD_AND_STR_OP(24, 0x72c, 0x00300000)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+	/* ESDCTL */
+	REG_LD_AND_STR_INIT(ESDCTL_BASE_ADDR)
+	REG_LD_AND_STR_OP(25, 0x088, 0x35343535)
+	REG_LD_AND_STR_OP(26, 0x090, 0x4d444c44)
+	REG_LD_AND_STR_OP(27, 0x07c, 0x01370138)
+	REG_LD_AND_STR_OP(28, 0x080, 0x013b013c)
+	REG_LD_AND_STR_OP(29, 0x018, 0x00001740)
+	REG_LD_AND_STR_OP(30, 0x000, 0xc3190000)
+	REG_LD_AND_STR_OP(31, 0x00c, 0x9f5152e3)
+	REG_LD_AND_STR_OP(32, 0x010, 0xb68e8a63)
+	REG_LD_AND_STR_OP(33, 0x014, 0x01ff00db)
+	REG_LD_AND_STR_OP(34, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(35, 0x030, 0x009f0e21)
+	REG_LD_AND_STR_OP(36, 0x008, 0x12273030)
+	REG_LD_AND_STR_OP(37, 0x004, 0x0002002d)
+	REG_LD_AND_STR_OP(38, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(39, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x00028031)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x052080b0)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x04008040)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x00028039)
+	REG_LD_AND_STR_OP(46, 0x01c, 0x05208138)
+	REG_LD_AND_STR_OP(47, 0x01c, 0x04008048)
+	REG_LD_AND_STR_OP(48, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(49, 0x040, 0x04b80003)
+	REG_LD_AND_STR_OP(50, 0x058, 0x00022227)
+	REG_LD_AND_STR_OP(51, 0x01C, 0x00000000)
+	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
+
+	/* For TO2 only, increase LD0 for VDIG_PLL to 1.3V */
+	REG_LD_AND_STR_INIT(IOMUXC_BASE_ADDR)
+	REG_LD_AND_STR_OP(52, 0x4, 0x00194005)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+/*
+ *  The following is to fill in those arguments for this ROM function
+ * pu_irom_hwcnfg_setup(void **start, size_t *bytes, const void *boot_data)
+ *
+ *  This function is used to copy data from the storage media into DDR.
+ *
+ *  start - Initial (possibly partial) image load address on entry. Final image
+ *            load address on exit.
+ *  bytes - Initial (possibly partial) image size on entry. Final image size on
+ *             exit.
+ *  boot_data - Initial @ref ivt Boot Data load address.
+ */
+	adr r0, DDR_DEST_ADDR
+	adr r1, COPY_SIZE
+	adr r2, BOOT_DATA
+before_calling_rom___pu_irom_hwcnfg_setup:
+	/* We should distinguish USB recovery mode(SDP mode) and internal boot mode.
+	If ROM runs in SDP mode, then it needn't load boot code from storage media.
+	If ROM runs in SDP mode, then r0 must be 0x00
+	If ROM runs in internal boot mode, then r0 should be larger than IRAM base address. */
+	cmp r7, #0xF8000000
+	bls return_sdp
+	/* Different ROM address for TO 1.0 & TO 2.0 */
+	ldr r3, =ROM_SI_REV
+	ldr r4, [r3]
+
+	cmp r4, #0x21
+	/* TO2.1 */
+	moveq r6, #0x1800
+	addeq r6, r6, #0x4d
+	beq 2f
+
+	cmp r4, #0x20
+	/* TO2 */
+	moveq r6, #0x1800
+	addeq r6, r6, #0x4d
+	beq 2f
+
+	/* TO1 */
+	mov r6, #0x400000
+	add r6, r6, #0x5000
+	add r6, r6, #0xc7
+2:
+	blx r6 /* This address might change in future ROM versions */
+after_calling_rom___pu_irom_hwcnfg_setup:
+
+return_sdp:
+	cmp r7, #0xF8000000
+	bhi quit_plugin
+
+	/* Workaround run plug-ins in SDP mode without USB re-enumeration.
+	how it works:
+	ROM running in usb download mode.
+	Host manufacturing application sends SDP command to download plug-in image.
+	Host manufacturing application sends SDP command to jump to plug-in image and run it.
+	Plug-in starts execution and after its regular tasks plug-in will then call into ROM
+	call into pl_parse_and_handle() */
+	ldr r3, =ROM_SI_REV
+	ldr r5, [r3]
+	cmp r5, #0x20       /* check silicon revision to determine the function entry address */
+
+	ldrlt r4, =0x00000edd   /* function entry in TO1 ROM */
+	ldrge r4, =0x0040487d   /* function entry in TO2 ROM */
+	blx r4
+
+/* To return to ROM from plugin, we need to fill in these argument.
+ * Here is what need to do:
+ * Need to construct the paramters for this function before return to ROM:
+ * plugin_download(void **start, size_t *bytes, UINT32 *ivt_offset)
+ */
+quit_plugin:
+	pop {r0-r6, lr}
+	ldr r4, DDR_DEST_ADDR
+	str r4, [r0]
+	ldr r4, COPY_SIZE
+	str r4, [r1]
+	mov r4, #0x400  /* Point to the second IVT table at offset 0x42C */
+	add r4, r4, #0x2C
+	str r4, [r2]
+	mov r0, #1
+
+	bx lr  /* return back to ROM code */
+
+DDR_DEST_ADDR:    .word   TEXT_BASE
+COPY_SIZE:        .word   _end - TEXT_BASE
+BOOT_DATA:        .word   TEXT_BASE
+                  .word   _end - TEXT_BASE
+                  .word   0
 
 #endif
diff --git a/board/freescale/mx53_smd/mx53_smd.c b/board/freescale/mx53_smd/mx53_smd.c
index eda4eec..0ced293 100644
--- a/board/freescale/mx53_smd/mx53_smd.c
+++ b/board/freescale/mx53_smd/mx53_smd.c
@@ -808,7 +808,7 @@ int board_init(void)
 #ifdef CONFIG_MFG
 /* MFG firmware need reset usb to avoid host crash firstly */
 #define USBCMD 0x140
-	int val = readl(OTG_BASE_ADDR + USBCMD);
+	val = readl(OTG_BASE_ADDR + USBCMD);
 	val &= ~0x1; /*RS bit*/
 	writel(val, OTG_BASE_ADDR + USBCMD);
 #endif