ENGR00155891 : Fix reboot stress test failed issue

If we replace DCD mode with plug-in mode in u-boot , we found DDR not stable.
We should enable "Force Measurement" after the delay line
parameters is configured in the plug-in code, for example:
0x63fd9088 = 0x34333936
0x63fd9090 = 0x49434942
0x63fd90F8 = 0x00000800 "Force Measurement"
update all of mx53 DDR script, include mx53_smd,mx53_loco,mx53_evk,mx53_ard,
mx53_pcba, at the same time, mx53_pcba will change from DCD mode to plug-in
mode in flash_header.S

Signed-off-by: Robin Gong <B38343@freescale.com>

5 files changed, 371 insertions, 258 deletions

diff --git a/board/freescale/mx53_ard/flash_header.S b/board/freescale/mx53_ard/flash_header.S
index ccb1019..fc563c3 100644
--- a/board/freescale/mx53_ard/flash_header.S
+++ b/board/freescale/mx53_ard/flash_header.S
@@ -110,29 +110,30 @@ plugin_start:
 	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_OP(29, 0x0f8, 0x00000800)
+	REG_LD_AND_STR_OP(30, 0x018, 0x00001740)
+	REG_LD_AND_STR_OP(31, 0x000, 0xc3190000)
+	REG_LD_AND_STR_OP(32, 0x00c, 0x9f5152e3)
+	REG_LD_AND_STR_OP(33, 0x010, 0xb68e8a63)
+	REG_LD_AND_STR_OP(34, 0x014, 0x01ff00db)
+	REG_LD_AND_STR_OP(35, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(36, 0x030, 0x009f0e21)
+	REG_LD_AND_STR_OP(37, 0x008, 0x12273030)
+	REG_LD_AND_STR_OP(38, 0x004, 0x0002002d)
+	REG_LD_AND_STR_OP(39, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x00028031)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x052080b0)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x04008040)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(46, 0x01c, 0x00028039)
+	REG_LD_AND_STR_OP(47, 0x01c, 0x05208138)
+	REG_LD_AND_STR_OP(48, 0x01c, 0x04008048)
+	REG_LD_AND_STR_OP(49, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(50, 0x040, 0x04b80003)
+	REG_LD_AND_STR_OP(51, 0x058, 0x00022227)
+	REG_LD_AND_STR_OP(52, 0x01C, 0x00000000)
 	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
 #else
 	/* IOMUX */
@@ -170,41 +171,42 @@ plugin_start:
 	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(30, 0x0f8, 0x00000800)
+	REG_LD_AND_STR_OP(31, 0x018, 0x00001710)
+	REG_LD_AND_STR_OP(32, 0x000, 0xc4110000)
+	REG_LD_AND_STR_OP(33, 0x00c, 0x4d5122d2)
+	REG_LD_AND_STR_OP(34, 0x010, 0x92d18a22)
+	REG_LD_AND_STR_OP(35, 0x014, 0x00c70092)
+	REG_LD_AND_STR_OP(36, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(37, 0x030, 0x009f000e)
+	REG_LD_AND_STR_OP(38, 0x008, 0x12272000)
+	REG_LD_AND_STR_OP(39, 0x004, 0x00030012)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x04008010)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x00008031)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x0b5280b0)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x04008010)
 	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(47, 0x01c, 0x00008020)
+	REG_LD_AND_STR_OP(48, 0x01c, 0x0a528030)
+	REG_LD_AND_STR_OP(49, 0x01c, 0x03c68031)
+	REG_LD_AND_STR_OP(50, 0x01c, 0x00448031)
+	REG_LD_AND_STR_OP(51, 0x01c, 0x04008018)
+	REG_LD_AND_STR_OP(52, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(53, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(54, 0x01c, 0x00008039)
+	REG_LD_AND_STR_OP(55, 0x01c, 0x0b528138)
+	REG_LD_AND_STR_OP(56, 0x01c, 0x04008018)
 	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_OP(58, 0x01c, 0x00008028)
+	REG_LD_AND_STR_OP(59, 0x01c, 0x0a528038)
+	REG_LD_AND_STR_OP(60, 0x01c, 0x03c68039)
+	REG_LD_AND_STR_OP(61, 0x01c, 0x00448039)
+	REG_LD_AND_STR_OP(62, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(63, 0x058, 0x00033335)
+	REG_LD_AND_STR_OP(64, 0x01c, 0x00000000)
+	REG_LD_AND_STR_OP(65, 0x040, 0x04b80003)
 	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
 #endif
 
@@ -282,13 +284,13 @@ return_sdp:
  */
 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]
+	ldr r7, DDR_DEST_ADDR
+	str r7, [r0]
+	ldr r7, COPY_SIZE
+	str r7, [r1]
+	mov r7, #0x400  /* Point to the second IVT table at offset 0x42C */
+	add r7, r7, #0x2C
+	str r7, [r2]
 	mov r0, #1
 
 	bx lr  /* return back to ROM code */
diff --git a/board/freescale/mx53_evk/flash_header.S b/board/freescale/mx53_evk/flash_header.S
index 6cadf3f..2fe3136 100644
--- a/board/freescale/mx53_evk/flash_header.S
+++ b/board/freescale/mx53_evk/flash_header.S
@@ -110,41 +110,42 @@ plugin_start:
 	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(30, 0x0f8, 0x00000800)
+	REG_LD_AND_STR_OP(31, 0x018, 0x00001710)
+	REG_LD_AND_STR_OP(32, 0x000, 0xc4110000)
+	REG_LD_AND_STR_OP(33, 0x00c, 0x4d5122d2)
+	REG_LD_AND_STR_OP(34, 0x010, 0x92d18a22)
+	REG_LD_AND_STR_OP(35, 0x014, 0x00c70092)
+	REG_LD_AND_STR_OP(36, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(37, 0x030, 0x009f000e)
+	REG_LD_AND_STR_OP(38, 0x008, 0x12272000)
+	REG_LD_AND_STR_OP(39, 0x004, 0x00030012)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x04008010)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x00008031)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x0b5280b0)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x04008010)
 	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(47, 0x01c, 0x00008020)
+	REG_LD_AND_STR_OP(48, 0x01c, 0x0a528030)
+	REG_LD_AND_STR_OP(49, 0x01c, 0x03c68031)
+	REG_LD_AND_STR_OP(50, 0x01c, 0x00448031)
+	REG_LD_AND_STR_OP(51, 0x01c, 0x04008018)
+	REG_LD_AND_STR_OP(52, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(53, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(54, 0x01c, 0x00008039)
+	REG_LD_AND_STR_OP(55, 0x01c, 0x0b528138)
+	REG_LD_AND_STR_OP(56, 0x01c, 0x04008018)
 	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_OP(58, 0x01c, 0x00008028)
+	REG_LD_AND_STR_OP(59, 0x01c, 0x0a528038)
+	REG_LD_AND_STR_OP(60, 0x01c, 0x03c68039)
+	REG_LD_AND_STR_OP(61, 0x01c, 0x00448039)
+	REG_LD_AND_STR_OP(62, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(63, 0x058, 0x00033335)
+	REG_LD_AND_STR_OP(64, 0x01c, 0x00000000)
+	REG_LD_AND_STR_OP(65, 0x040, 0x04b80003)
 	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
 
 /*
@@ -221,13 +222,13 @@ return_sdp:
  */
 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]
+	ldr r7, DDR_DEST_ADDR
+	str r7, [r0]
+	ldr r7, COPY_SIZE
+	str r7, [r1]
+	mov r7, #0x400  /* Point to the second IVT table at offset 0x42C */
+	add r7, r7, #0x2C
+	str r7, [r2]
 	mov r0, #1
 
 	bx lr  /* return back to ROM code */
diff --git a/board/freescale/mx53_loco/flash_header.S b/board/freescale/mx53_loco/flash_header.S
index 5a9dc0f..b3dc1d4 100644
--- a/board/freescale/mx53_loco/flash_header.S
+++ b/board/freescale/mx53_loco/flash_header.S
@@ -109,29 +109,30 @@ plugin_start:
 	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_OP(29, 0x0f8, 0x00000800)
+	REG_LD_AND_STR_OP(30, 0x018, 0x00001740)
+	REG_LD_AND_STR_OP(31, 0x000, 0xc3190000)
+	REG_LD_AND_STR_OP(32, 0x00c, 0x9f5152e3)
+	REG_LD_AND_STR_OP(33, 0x010, 0xb68e8a63)
+	REG_LD_AND_STR_OP(34, 0x014, 0x01ff00db)
+	REG_LD_AND_STR_OP(35, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(36, 0x030, 0x009f0e21)
+	REG_LD_AND_STR_OP(37, 0x008, 0x12273030)
+	REG_LD_AND_STR_OP(38, 0x004, 0x0002002d)
+	REG_LD_AND_STR_OP(39, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x00028031)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x052080b0)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x04008040)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(46, 0x01c, 0x00028039)
+	REG_LD_AND_STR_OP(47, 0x01c, 0x05208138)
+	REG_LD_AND_STR_OP(48, 0x01c, 0x04008048)
+	REG_LD_AND_STR_OP(49, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(50, 0x040, 0x04b80003)
+	REG_LD_AND_STR_OP(51, 0x058, 0x00022227)
+	REG_LD_AND_STR_OP(52, 0x01C, 0x00000000)
 	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
 
 /*
@@ -146,6 +147,7 @@ plugin_start:
  *             exit.
  *  boot_data - Initial @ref ivt Boot Data load address.
  */
+
 	adr r0, DDR_DEST_ADDR
 	adr r1, COPY_SIZE
 	adr r2, BOOT_DATA
@@ -208,13 +210,13 @@ return_sdp:
  */
 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]
+	ldr r7, DDR_DEST_ADDR
+	str r7, [r0]
+	ldr r7, COPY_SIZE
+	str r7, [r1]
+	mov r7, #0x400  /* Point to the second IVT table at offset 0x42C */
+	add r7, r7, #0x2C
+	str r7, [r2]
 	mov r0, #1
 
 	bx lr  /* return back to ROM code */
diff --git a/board/freescale/mx53_pcba/flash_header.S b/board/freescale/mx53_pcba/flash_header.S
index 68778f4..da12348 100644
--- a/board/freescale/mx53_pcba/flash_header.S
+++ b/board/freescale/mx53_pcba/flash_header.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2010-2011 Freescale Semiconductor, Inc.
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -20,106 +20,213 @@
 #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 0x40B001D2 /* Tag=0xD2, Len=53*8 + 4 + 4, Ver=0x40 */
-write_dcd_cmd:    .word 0x04AC01CC /* Tag=0xCC, Len=53*8 + 4, Param=4 */
-
-/* DCD */
-MXC_DCD_ITEM(1, IOMUXC_BASE_ADDR + 0x6f4, 0x00000000)
-MXC_DCD_ITEM(2, IOMUXC_BASE_ADDR + 0x714, 0x00000000)
-MXC_DCD_ITEM(3, IOMUXC_BASE_ADDR + 0x6fc, 0x00000000)
-MXC_DCD_ITEM(4, IOMUXC_BASE_ADDR + 0x724, 0x04000000)
-
-MXC_DCD_ITEM(5, IOMUXC_BASE_ADDR + 0x72c, 0x00300000)
-MXC_DCD_ITEM(6, IOMUXC_BASE_ADDR + 0x554, 0x00300000)
-MXC_DCD_ITEM(7, IOMUXC_BASE_ADDR + 0x558, 0x00300040)
-MXC_DCD_ITEM(8, IOMUXC_BASE_ADDR + 0x728, 0x00300000)
-
-MXC_DCD_ITEM(9, IOMUXC_BASE_ADDR + 0x560, 0x00300000)
-MXC_DCD_ITEM(10, IOMUXC_BASE_ADDR + 0x568, 0x00300040)
-MXC_DCD_ITEM(11, IOMUXC_BASE_ADDR + 0x71c, 0x00300000)
-MXC_DCD_ITEM(12, IOMUXC_BASE_ADDR + 0x594, 0x00300000)
-
-MXC_DCD_ITEM(13, IOMUXC_BASE_ADDR + 0x590, 0x00300040)
-MXC_DCD_ITEM(14, IOMUXC_BASE_ADDR + 0x718, 0x00300000)
-MXC_DCD_ITEM(15, IOMUXC_BASE_ADDR + 0x584, 0x00300000)
-MXC_DCD_ITEM(16, IOMUXC_BASE_ADDR + 0x57c, 0x00300040)
-
-MXC_DCD_ITEM(17, IOMUXC_BASE_ADDR + 0x578, 0x00300000)
-MXC_DCD_ITEM(18, IOMUXC_BASE_ADDR + 0x570, 0x00300000)
-MXC_DCD_ITEM(19, IOMUXC_BASE_ADDR + 0x574, 0x00300000)
-MXC_DCD_ITEM(20, IOMUXC_BASE_ADDR + 0x588, 0x00300000)
-
-MXC_DCD_ITEM(21, IOMUXC_BASE_ADDR + 0x6f0, 0x00300000)
-MXC_DCD_ITEM(22, IOMUXC_BASE_ADDR + 0x720, 0x00300000)
-MXC_DCD_ITEM(23, IOMUXC_BASE_ADDR + 0x564, 0x00300040)
-MXC_DCD_ITEM(24, IOMUXC_BASE_ADDR + 0x580, 0x00300040)
-
-MXC_DCD_ITEM(25, ESDCTL_BASE_ADDR + 0x098, 0x00000000)
-MXC_DCD_ITEM(26, ESDCTL_BASE_ADDR + 0x07c, 0x01340135)
-MXC_DCD_ITEM(27, ESDCTL_BASE_ADDR + 0x080, 0x01390139)
-MXC_DCD_ITEM(28, ESDCTL_BASE_ADDR + 0x088, 0x34343531)
-
-MXC_DCD_ITEM(29, ESDCTL_BASE_ADDR + 0x090, 0x54475246)
-MXC_DCD_ITEM(30, ESDCTL_BASE_ADDR + 0x018, 0x00011740)
-MXC_DCD_ITEM(31, ESDCTL_BASE_ADDR + 0x000, 0xc3190000)
-MXC_DCD_ITEM(32, ESDCTL_BASE_ADDR + 0x00c, 0x9f515333)
-
-MXC_DCD_ITEM(33, ESDCTL_BASE_ADDR + 0x010, 0xb68e8a63)
-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, 0x12373030)
-MXC_DCD_ITEM(38, ESDCTL_BASE_ADDR + 0x004, 0x4402002d)
-MXC_DCD_ITEM(39, ESDCTL_BASE_ADDR + 0x01c, 0x00008032)
-MXC_DCD_ITEM(40, ESDCTL_BASE_ADDR + 0x01c, 0x00008033)
-
-MXC_DCD_ITEM(41, ESDCTL_BASE_ADDR + 0x01c, 0x00008031)
-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, 0x0000803a)
-
-MXC_DCD_ITEM(45, ESDCTL_BASE_ADDR + 0x01c, 0x0000803b)
-MXC_DCD_ITEM(46, ESDCTL_BASE_ADDR + 0x01c, 0x00028039)
-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, 0x00022227)
-MXC_DCD_ITEM(52, ESDCTL_BASE_ADDR + 0x01C, 0x00000000)
-
-MXC_DCD_ITEM(53, IOMUXC_BASE_ADDR + 0x004, 0x00194005)
+
+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, 0x6f4, 0x00000000)
+	REG_LD_AND_STR_OP(2, 0x714, 0x00000000)
+	REG_LD_AND_STR_OP(3, 0x6fc, 0x00000000)
+	REG_LD_AND_STR_OP(4, 0x724, 0x04000000)
+	REG_LD_AND_STR_OP(5, 0x72c, 0x00300000)
+	REG_LD_AND_STR_OP(6, 0x554, 0x00300000)
+	REG_LD_AND_STR_OP(7, 0x558, 0x00300040)
+	REG_LD_AND_STR_OP(8, 0x728, 0x00300000)
+	REG_LD_AND_STR_OP(9, 0x560, 0x00300000)
+	REG_LD_AND_STR_OP(10, 0x568, 0x00300040)
+	REG_LD_AND_STR_OP(11, 0x71c, 0x00300000)
+	REG_LD_AND_STR_OP(12, 0x594, 0x00300000)
+	REG_LD_AND_STR_OP(13, 0x590, 0x00300040)
+	REG_LD_AND_STR_OP(14, 0x718, 0x00300000)
+	REG_LD_AND_STR_OP(15, 0x584, 0x00300000)
+	REG_LD_AND_STR_OP(16, 0x57c, 0x00300040)
+	REG_LD_AND_STR_OP(17, 0x578, 0x00300000)
+	REG_LD_AND_STR_OP(18, 0x570, 0x00300000)
+	REG_LD_AND_STR_OP(19, 0x574, 0x00300000)
+	REG_LD_AND_STR_OP(20, 0x588, 0x00300000)
+	REG_LD_AND_STR_OP(21, 0x6f0, 0x00300000)
+	REG_LD_AND_STR_OP(22, 0x720, 0x00300000)
+	REG_LD_AND_STR_OP(23, 0x564, 0x00300040)
+	REG_LD_AND_STR_OP(24, 0x580, 0x00300040)
+	REG_LD_AND_STR_END(IOMUXC_BASE_ADDR)
+
+	/* ESDCTL */
+	REG_LD_AND_STR_INIT(ESDCTL_BASE_ADDR)
+	REG_LD_AND_STR_OP(25, 0x098, 0x00000000)
+	REG_LD_AND_STR_OP(26, 0x07c, 0x01340135)
+	REG_LD_AND_STR_OP(27, 0x080, 0x01390139)
+	REG_LD_AND_STR_OP(28, 0x0f8, 0x00000800)
+	REG_LD_AND_STR_OP(29, 0x088, 0x34343531)
+	REG_LD_AND_STR_OP(30, 0x090, 0x54475246)
+	REG_LD_AND_STR_OP(31, 0x018, 0x00011740)
+	REG_LD_AND_STR_OP(32, 0x000, 0xc3190000)
+	REG_LD_AND_STR_OP(33, 0x00c, 0x9f515333)
+	REG_LD_AND_STR_OP(34, 0x010, 0xb68e8a63)
+	REG_LD_AND_STR_OP(35, 0x014, 0x01ff00db)
+	REG_LD_AND_STR_OP(36, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(37, 0x030, 0x009f0e21)
+	REG_LD_AND_STR_OP(38, 0x008, 0x12373030)
+	REG_LD_AND_STR_OP(39, 0x004, 0x4402002d)
+	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, 0x092080b0)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x04008040)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(46, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(47, 0x01c, 0x00028039)
+	REG_LD_AND_STR_OP(48, 0x01c, 0x09208138)
+	REG_LD_AND_STR_OP(49, 0x01c, 0x04008048)
+	REG_LD_AND_STR_OP(50, 0x020, 0x00001800)
+	REG_LD_AND_STR_OP(51, 0x040, 0x04b80003)
+	REG_LD_AND_STR_OP(52, 0x058, 0x00022227)
+	REG_LD_AND_STR_OP(53, 0x01C, 0x00000000)
+	REG_LD_AND_STR_OP(54, 0x004, 0x00194005)
+	REG_LD_AND_STR_END(ESDCTL_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 r7, DDR_DEST_ADDR
+	str r7, [r0]
+	ldr r7, COPY_SIZE
+	str r7, [r1]
+	mov r7, #0x400  /* Point to the second IVT table at offset 0x42C */
+	add r7, r7, #0x2C
+	str r7, [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 0411f1b..8a1e091 100644
--- a/board/freescale/mx53_smd/flash_header.S
+++ b/board/freescale/mx53_smd/flash_header.S
@@ -109,29 +109,30 @@ plugin_start:
 	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_OP(29, 0x0f8, 0x00000800)
+	REG_LD_AND_STR_OP(30, 0x018, 0x00001740)
+	REG_LD_AND_STR_OP(31, 0x000, 0xc3190000)
+	REG_LD_AND_STR_OP(32, 0x00c, 0x9f5152e3)
+	REG_LD_AND_STR_OP(33, 0x010, 0xb68e8a63)
+	REG_LD_AND_STR_OP(34, 0x014, 0x01ff00db)
+	REG_LD_AND_STR_OP(35, 0x02c, 0x000026d2)
+	REG_LD_AND_STR_OP(36, 0x030, 0x009f0e21)
+	REG_LD_AND_STR_OP(37, 0x008, 0x12273030)
+	REG_LD_AND_STR_OP(38, 0x004, 0x0002002d)
+	REG_LD_AND_STR_OP(39, 0x01c, 0x00008032)
+	REG_LD_AND_STR_OP(40, 0x01c, 0x00008033)
+	REG_LD_AND_STR_OP(41, 0x01c, 0x00028031)
+	REG_LD_AND_STR_OP(42, 0x01c, 0x052080b0)
+	REG_LD_AND_STR_OP(43, 0x01c, 0x04008040)
+	REG_LD_AND_STR_OP(44, 0x01c, 0x0000803a)
+	REG_LD_AND_STR_OP(45, 0x01c, 0x0000803b)
+	REG_LD_AND_STR_OP(46, 0x01c, 0x00028039)
+	REG_LD_AND_STR_OP(47, 0x01c, 0x05208138)
+	REG_LD_AND_STR_OP(48, 0x01c, 0x04008048)
+	REG_LD_AND_STR_OP(49, 0x020, 0x00005800)
+	REG_LD_AND_STR_OP(50, 0x040, 0x04b80003)
+	REG_LD_AND_STR_OP(51, 0x058, 0x00022227)
+	REG_LD_AND_STR_OP(52, 0x01C, 0x00000000)
 	REG_LD_AND_STR_END(ESDCTL_BASE_ADDR)
 
 /*
@@ -206,13 +207,13 @@ return_sdp:
  */
 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]
+	ldr r7, DDR_DEST_ADDR
+	str r7, [r0]
+	ldr r7, COPY_SIZE
+	str r7, [r1]
+	mov r7, #0x400  /* Point to the second IVT table at offset 0x42C */
+	add r7, r7, #0x2C
+	str r7, [r2]
 	mov r0, #1
 
 	bx lr  /* return back to ROM code */