ENGR00025557: MX50 Add ZQ calibration support for TO1.1.1.

MX50 Add ZQ calibration support for TO1.1.1.
This need to be enabled by CONFIG_ZQ_CALIB.

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

2 files changed, 487 insertions, 79 deletions

diff --git a/board/freescale/mx50_arm2/flash_header.S b/board/freescale/mx50_arm2/flash_header.S
index 8bdc530..30545a7 100644
--- a/board/freescale/mx50_arm2/flash_header.S
+++ b/board/freescale/mx50_arm2/flash_header.S
@@ -25,6 +25,159 @@
 # error "Must define the offset of flash header"
 #endif
 
+#if defined(CONFIG_ZQ_CALIB)
+.macro do_zq_calib
+/*=============================================================================
+ * ZQ calibration
+ *===========================================================================*/
+	ldr r0, =DATABAHN_BASE_ADDR
+
+/*=============================================================================
+ * Pu calibration start
+ *===========================================================================*/
+	ldr r1, =0x0
+pu_loop:
+	cmp r1, #0x20
+	beq pu_out
+
+	// pu<<16,pd=0
+	mov r2, r1, lsl #16
+	// Set SW_CFG1
+	str r2, [r0, #0x128]
+
+	// ((pd+1)<<8) | (pu+1)
+	add r1, r1, #0x1
+	ldr r2, =0x1
+	orr r3, r1, r2, lsl #8
+	// Set SW_CFG2
+	str r3, [r0, #0x12c]
+
+	sub r1, r1, #0x1
+
+	// Start ZQ comparator
+	ldr r2, =0x10000
+	str r2, [r0, #0x124]
+
+	// Delay 300ns at least
+	ldr r3, =0x0
+pu_delay:
+	add r3, r3, #0x1
+	cmp r3, #0x1000
+	bne pu_delay
+
+	// Read compare result
+	ldr r3, [r0,#0x14c]
+	and r3, r3, #0x1
+
+	// Stop ZQ comparator
+	ldr r2, =0x0
+	str r2, [r0, #0x124]
+
+	// Add pu value
+	add r1, r1, #0x1
+	cmp r3, #0x1
+	bne pu_loop
+
+pu_out:
+	// Pu calibration result in r1
+	sub r1, r1, #0x1
+
+/*=============================================================================
+ * PD calibration start
+ *===========================================================================*/
+	ldr r2, =0x0
+pd_loop:
+	cmp r2, #0xf
+	beq pd_out
+
+	// pd<<24 | pu<<16 | 1<<4
+	mov r3, r2, lsl #24
+	orr r3, r3, r1, lsl #16
+	orr r3, r3, #0x10
+	// Set SW_CFG1
+	str r3, [r0, #0x128]
+
+	// ((pd+1) << 8) | (pu+1)
+	add r2, r2, #0x1
+	add r1, r1, #0x1
+	mov r3, r2, LSL #8
+	orr r3, r3, r1
+	// Set SW_CFG2
+	str r3, [r0, #0x12c]
+
+	sub r2, r2, #0x1
+	sub r1, r1, #0x1
+
+	// Start ZQ comparator
+	ldr r3, =0x10000
+	str r3, [r0, #0x124]
+
+	// Delay 300ns at least
+	ldr r3, =0x0
+pd_delay:
+	add r3, r3, #0x1
+	cmp r3, #0x1000
+	bne pd_delay
+
+	// Read compare result
+	ldr r3, [r0, #0x14c]
+	and r3, r3, #0x1
+
+	// Stop ZQ comparator
+	ldr r8, =0x0
+	str r8, [r0, #0x124]
+
+	// Add pd value
+	add r2, r2, #0x1
+
+	cmp r3, #0x0
+	bne pd_loop
+
+	// Make sure r2 > 0
+	cmp r2, #0x0
+	beq pd_loop
+
+pd_out:
+	// Pd calibration result in r2
+	sub r2, r2, #0x2
+
+/*=============================================================================
+ * Software load PU/PD value,PU is stored in r1, and PD is stored in r2
+ *===========================================================================*/
+	/* ((pd + 1) << 24) | ((pu + 1) << 16) */
+	add r5, r1, #0x1
+	add r6, r2, #0x1
+	mov r3, r6, lsl #24
+	orr r3, r3, r5, lsl #16
+	/* Set SW_CFG1 */
+	str r3, [r0, #0x128]
+
+	/* (pd << 8) | pu */
+	mov r3, r2, lsl #8
+	orr r3, r3, r1
+	/* Set SW_CFG2 */
+	str r3, [r0, #0x12c]
+
+	/* Loading PU value, set pu_pd_sel=0 */
+	ldr r3, =0x310000
+	str r3, [r0, #0x124]
+	/* Clear for next load */
+	ldr r3, =0x200000
+	str r3, [r0, #0x124]
+
+	/* Loading PD value, set pu_pd_sel=1 */
+	ldr r3, =0x10
+	orr r3, r3, r6, lsl #24
+	orr r3, r3, r5, lsl #16
+	str r3, [r0, #0x128]
+
+	ldr r3, =0x310000
+	str r3, [r0, #0x124]
+	ldr r3, =0x200000
+	str r3, [r0, #0x124]
+.endm
+#endif
+
 .section ".text.flasheader", "x"
 	b	_start
 	.org	CONFIG_FLASH_HEADER_OFFSET
@@ -62,7 +215,24 @@ plugin2:           .long 0x0
 plugin_start:
 
 /* Save the return address and the function arguments */
-	push    {r0-r2, lr}
+	push    {r0-r6, r8, lr}
+
+/* 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)
+ */
+ /* Check r0 if valid address
+  * Mfgtools or sb_loader run-plug maybe pass down invalidate data
+ */
+ 	cmp r0, #0xf8000000
+	mov r4, r0
+ 	ldr r3, DDR_DEST_ADDR
+	strhi r3, [r0]
+	ldr r3, COPY_SIZE
+	strhi r3, [r1]
+	ldr r3, IVT2_HDR_OFS
+	strhi r3, [r2]
 
 /*=============================================================================
  *init script for codex LPDDR1-200MHz CPU board
@@ -137,9 +307,9 @@ wait_pll1_lock:
  * IOMUX
  *===========================================================================*/
 	ldr r0, =0x53fa8000
-	mov r1, #0x02000000
+	mov r1, #0x04000000
 	str r1, [r0, #0x6ac]
-	mov r2, #0x00200000
+	ldr r2, =0x00180000
 	str r2, [r0, #0x6a4]
 	str r2, [r0, #0x668]
 	str r2, [r0, #0x698]
@@ -157,11 +327,44 @@ wait_pll1_lock:
         str r2, [r0, #0x4cc]
         str r2, [r0, #0x524]
 
+//*===========================================
+//   DDR controller setting
+//*===========================================
+// CTL setting
+	ldr r0, =DATABAHN_BASE_ADDR
+
+#if defined(CONFIG_ZQ_CALIB)
+	do_zq_calib
+#else
+// setmem /32 0x1400012C = 0x00000817  // pd=<<8, pu=<<0
+	ldr r1, =0x00000817
+	str r1, [r0, #0x12c]
+// setmem /32 0x14000128 = 0x09180000  // (pd+1)<<24, (pu+1)<<16
+	ldr r1, =0x09180000
+	str r1, [r0, #0x128]
+// load PU, pu_pd_sel=0
+// setmem /32 0x14000124 = 0x00310000  // software load ZQ: 3<<20, 1<<16
+	ldr r1, =0x00310000
+	str r1, [r0, #0x124]
+// setmem /32 0x14000124 = 0x00200000  // clear for next load
+	ldr r1, =0x00200000
+	str r1, [r0, #0x124]
+// load PD, pu_pd_sel=1
+// setmem /32 0x14000128 = 0x09180010  // (pd+1)<<24, (pu+1)<<16, 1<<4
+	ldr r1, =0x09180010
+	str r1, [r0, #0x128]
+// setmem /32 0x14000124 = 0x00310000  // software load ZQ: 3<<20, 1<<16
+	ldr r1, =0x00310000
+	str r1, [r0, #0x124]
+// setmem /32 0x14000124 = 0x00200000  // clear for next load
+	ldr r1, =0x00200000
+	str r1, [r0, #0x124]
+#endif
+
 /*=============================================================================
  * DDR setting
  *===========================================================================*/
 
-	ldr r0, =DATABAHN_BASE_ADDR
 /* setmem /32 0x14000000	= 0x00000500 */
 	ldr r1, =0x00000500
 	str r1, [r0, #0x0]
@@ -348,8 +551,8 @@ wait_pll1_lock:
 /* setmem /32 0x14000118	= 0x01010002 */
         ldr r1, =0x01010002
         str r1, [r0, #0x118]
-/* setmem /32 0x1400011c	= 0x00000000 */
-        ldr r1, =0x00000000
+/* setmem /32 0x1400011c	= 0x00001000 */
+        ldr r1, =0x00001000
         str r1, [r0, #0x11c]
 /* setmem /32 0x14000200	= 0x00000000 */
 	ldr r1, =0x00000000
@@ -490,8 +693,39 @@ wait_pll1_lock:
 	str r3, [r0, #0x24]
 
 /*	---------- DDR SETUP ---------- */
-/*	ldr r0, =CSP_BASE_REG_PA_DATABAHN   */
-	ldr r0, =0x14000000
+//*===========================================
+//   DDR controller setting
+//*===========================================
+// CTL setting
+	ldr r0, =DATABAHN_BASE_ADDR
+
+#if defined(CONFIG_ZQ_CALIB)
+	do_zq_calib
+#else
+// setmem /32 0x1400012C = 0x00000817  // pd=<<8, pu=<<0
+	ldr r1, =0x00000817
+	str r1, [r0, #0x12c]
+// setmem /32 0x14000128 = 0x09180000  // (pd+1)<<24, (pu+1)<<16
+	ldr r1, =0x09180000
+	str r1, [r0, #0x128]
+// load PU, pu_pd_sel=0
+// setmem /32 0x14000124 = 0x00310000  // software load ZQ: 3<<20, 1<<16
+	ldr r1, =0x00310000
+	str r1, [r0, #0x124]
+// setmem /32 0x14000124 = 0x00200000  // clear for next load
+	ldr r1, =0x00200000
+	str r1, [r0, #0x124]
+// load PD, pu_pd_sel=1
+// setmem /32 0x14000128 = 0x09180010  // (pd+1)<<24, (pu+1)<<16, 1<<4
+	ldr r1, =0x09180010
+	str r1, [r0, #0x128]
+// setmem /32 0x14000124 = 0x00310000  // software load ZQ: 3<<20, 1<<16
+	ldr r1, =0x00310000
+	str r1, [r0, #0x124]
+// setmem /32 0x14000124 = 0x00200000  // clear for next load
+	ldr r1, =0x00200000
+	str r1, [r0, #0x124]
+#endif
 
 /*	CTL setting */
 /*	setmem /32 0x14000000    = 0x00000400 */
@@ -707,8 +941,8 @@ wait_pll1_lock:
 /*	setmem /32 0x14000118    = 0x01010002 */
 	ldr r1, =0x01010002
 	str r1, [r0, #0x118]
-/*	setmem /32 0x1400011c    = 0x00000000 */
-	ldr r1, =0x00000000
+/*	setmem /32 0x1400011c    = 0x00001000 */
+	ldr r1, =0x00001000
 	str r1, [r0, #0x11c]
 /*	setmem /32 0x14000120    = 0x00000000 */
 	ldr r1, =0x00000000
@@ -862,8 +1096,8 @@ wait_pll1_lock:
  * IOMUX
  *=================================================================*/
 	ldr r0, =0x53fa8600
-	mov r1, #0x02000000
-	mov r3, #0x00200000
+	mov r1, #0x00000000
+	mov r3, #0x00380000
 	mov r2, #0x0
 	str r1, [r0, #0xac]
 	str r2, [r0, #0x6c]
@@ -888,10 +1122,43 @@ wait_pll1_lock:
 	str r3, [r0, #0xcc]
 	str r3, [r0, #0x124]
 
+//*===========================================
+//   DDR controller setting
+//*===========================================
+// CTL setting
+	ldr r0, =DATABAHN_BASE_ADDR
+
+#if defined(CONFIG_ZQ_CALIB)
+	do_zq_calib
+#else
+// setmem /32 0x1400012C = 0x00000817  // pd=<<8, pu=<<0
+	ldr r1, =0x00000817
+	str r1, [r0, #0x12c]
+// setmem /32 0x14000128 = 0x09180000  // (pd+1)<<24, (pu+1)<<16
+	ldr r1, =0x09180000
+	str r1, [r0, #0x128]
+// load PU, pu_pd_sel=0
+// setmem /32 0x14000124 = 0x00310000  // software load ZQ: 3<<20, 1<<16
+	ldr r1, =0x00310000
+	str r1, [r0, #0x124]
+// setmem /32 0x14000124 = 0x00200000  // clear for next load
+	ldr r1, =0x00200000
+	str r1, [r0, #0x124]
+// load PD, pu_pd_sel=1
+// setmem /32 0x14000128 = 0x09180010  // (pd+1)<<24, (pu+1)<<16, 1<<4
+	ldr r1, =0x09180010
+	str r1, [r0, #0x128]
+// setmem /32 0x14000124 = 0x00310000  // software load ZQ: 3<<20, 1<<16
+	ldr r1, =0x00310000
+	str r1, [r0, #0x124]
+// setmem /32 0x14000124 = 0x00200000  // clear for next load
+	ldr r1, =0x00200000
+	str r1, [r0, #0x124]
+#endif
+
 /*==============================================================
  *  DDR setting
  *=============================================================*/
-	ldr r0, =DATABAHN_BASE_ADDR
 /* setmem /32 0x14000000 = 0x00000100 */
 	ldr r1, =0x00000100
 	str r1, [r0, #0x0]
@@ -1139,6 +1406,7 @@ wait_pll1_lock:
  * Final image size on exit.
  * boot_data - Initial @ref ivt Boot Data load address.
  */
+
 	ldr r0, =ROM_SI_REV
 	ldr r1, [r0]
 	cmp r1, #0x11
@@ -1148,34 +1416,23 @@ wait_pll1_lock:
 	adr r2, BOOT_DATA
 
 before_calling_rom___pu_irom_hwcnfg_setup:
-	mov r4, #0x2a00
+	mov r3, #0x2a00
 	/* Different ROM address for TO 1.0 & TO 1.1.1 */
-	addeq r4, r4, #0xad
-	addne r4, r4, #0x19
-	blx r4
+	addeq r3, r3, #0xad
+	addne r3, r3, #0x19
+	cmp r4, #0xf8000000
+	blxhi r3
 after_calling_rom___pu_irom_hwcnfg_setup:
 
-
-/* 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)
- */
-	pop {r0-r2, lr}
-	ldr r3, DDR_DEST_ADDR
-	str r3, [r0]
-	ldr r3, COPY_SIZE
-	str r3, [r1]
-	mov r3, #0x400  /* Point to the second IVT table at offset 0x42C */
-	add r3, r3, #0x2C
-	str r3, [r2]
+	pop {r0-r6, r8, lr}
 	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 /*data be copied by pu_irom_hwcnfg_setup()*/
-                  .word   0
+DDR_DEST_ADDR:		.word TEXT_BASE
+COPY_SIZE:		.word _end - TEXT_BASE
+BOOT_DATA:		.word TEXT_BASE
+			.word _end - TEXT_BASE /*data be copied by pu_irom_hwcnfg_setup()*/
+			.word 0
+IVT2_HDR_OFS:           .word ivt2_header - TEXT_BASE
 
 #endif
diff --git a/board/freescale/mx50_rdp/flash_header.S b/board/freescale/mx50_rdp/flash_header.S
index 51ee20a..7a547d9 100644
--- a/board/freescale/mx50_rdp/flash_header.S
+++ b/board/freescale/mx50_rdp/flash_header.S
@@ -25,6 +25,159 @@
 # error "Must define the offset of flash header"
 #endif
 
+#if defined(CONFIG_ZQ_CALIB)
+.macro do_zq_calib
+/*=============================================================================
+ * ZQ calibration
+ *===========================================================================*/
+	ldr r0, =DATABAHN_BASE_ADDR
+
+/*=============================================================================
+ * Pu calibration start
+ *===========================================================================*/
+	ldr r1, =0x0
+pu_loop:
+	cmp r1, #0x20
+	beq pu_out
+
+	// pu<<16,pd=0
+	mov r2, r1, lsl #16
+	// Set SW_CFG1
+	str r2, [r0, #0x128]
+
+	// ((pd+1)<<8) | (pu+1)
+	add r1, r1, #0x1
+	ldr r2, =0x1
+	orr r3, r1, r2, lsl #8
+	// Set SW_CFG2
+	str r3, [r0, #0x12c]
+
+	sub r1, r1, #0x1
+
+	// Start ZQ comparator
+	ldr r2, =0x10000
+	str r2, [r0, #0x124]
+
+	// Delay 300ns at least
+	ldr r3, =0x0
+pu_delay:
+	add r3, r3, #0x1
+	cmp r3, #0x1000
+	bne pu_delay
+
+	// Read compare result
+	ldr r3, [r0,#0x14c]
+	and r3, r3, #0x1
+
+	// Stop ZQ comparator
+	ldr r2, =0x0
+	str r2, [r0, #0x124]
+
+	// Add pu value
+	add r1, r1, #0x1
+	cmp r3, #0x1
+	bne pu_loop
+
+pu_out:
+	// Pu calibration result in r1
+	sub r1, r1, #0x1
+
+/*=============================================================================
+ * PD calibration start
+ *===========================================================================*/
+	ldr r2, =0x0
+pd_loop:
+	cmp r2, #0xf
+	beq pd_out
+
+	// pd<<24 | pu<<16 | 1<<4
+	mov r3, r2, lsl #24
+	orr r3, r3, r1, lsl #16
+	orr r3, r3, #0x10
+	// Set SW_CFG1
+	str r3, [r0, #0x128]
+
+	// ((pd+1) << 8) | (pu+1)
+	add r2, r2, #0x1
+	add r1, r1, #0x1
+	mov r3, r2, LSL #8
+	orr r3, r3, r1
+	// Set SW_CFG2
+	str r3, [r0, #0x12c]
+
+	sub r2, r2, #0x1
+	sub r1, r1, #0x1
+
+	// Start ZQ comparator
+	ldr r3, =0x10000
+	str r3, [r0, #0x124]
+
+	// Delay 300ns at least
+	ldr r3, =0x0
+pd_delay:
+	add r3, r3, #0x1
+	cmp r3, #0x1000
+	bne pd_delay
+
+	// Read compare result
+	ldr r3, [r0, #0x14c]
+	and r3, r3, #0x1
+
+	// Stop ZQ comparator
+	ldr r8, =0x0
+	str r8, [r0, #0x124]
+
+	// Add pd value
+	add r2, r2, #0x1
+
+	cmp r3, #0x0
+	bne pd_loop
+
+	// Make sure r2 > 0
+	cmp r2, #0x0
+	beq pd_loop
+
+pd_out:
+	// Pd calibration result in r2
+	sub r2, r2, #0x2
+
+/*=============================================================================
+ * Software load PU/PD value,PU is stored in r1, and PD is stored in r2
+ *===========================================================================*/
+	/* ((pd + 1) << 24) | ((pu + 1) << 16) */
+	add r5, r1, #0x1
+	add r6, r2, #0x1
+	mov r3, r6, lsl #24
+	orr r3, r3, r5, lsl #16
+	/* Set SW_CFG1 */
+	str r3, [r0, #0x128]
+
+	/* (pd << 8) | pu */
+	mov r3, r2, lsl #8
+	orr r3, r3, r1
+	/* Set SW_CFG2 */
+	str r3, [r0, #0x12c]
+
+	/* Loading PU value, set pu_pd_sel=0 */
+	ldr r3, =0x310000
+	str r3, [r0, #0x124]
+	/* Clear for next load */
+	ldr r3, =0x200000
+	str r3, [r0, #0x124]
+
+	/* Loading PD value, set pu_pd_sel=1 */
+	ldr r3, =0x10
+	orr r3, r3, r6, lsl #24
+	orr r3, r3, r5, lsl #16
+	str r3, [r0, #0x128]
+
+	ldr r3, =0x310000
+	str r3, [r0, #0x124]
+	ldr r3, =0x200000
+	str r3, [r0, #0x124]
+.endm
+#endif
+
 .section ".text.flasheader", "x"
 	b	_start
 	.org	CONFIG_FLASH_HEADER_OFFSET
@@ -61,7 +214,7 @@ plugin2:           .long 0x0
 
 plugin_start:
 /* Save the return address and the function arguments */
-	push    {r0-r4, lr}
+	push    {r0-r6, r8, lr}
 
 /* To return to ROM from plugin, we need to fill in these argument.
  * Here is what need to do:
@@ -77,8 +230,7 @@ plugin_start:
 	strhi r3, [r0]
 	ldr r3, COPY_SIZE
 	strhi r3, [r1]
-	mov r3, #0x400  /* Point to the second IVT table at offset 0x42C */
-	add r3, r3, #0x2C
+	ldr r3, IVT2_HDR_OFS
 	strhi r3, [r2]
 
 /*=============================================================================
@@ -153,7 +305,7 @@ wait_pll1_lock:
  *===========================================================================*/
     ldr r0, =0x53fa8600
     mov r1, #0x04000000
-    mov r3, #0x00180000
+    ldr r3, =0x00180000
     mov r2, #0x0
 //setmem /32 0x53fa86ac = 0x04000000
 //IOMUXC_SW_PAD_CTL_GRP_DDR_TYPE, ddr_sel=2'b01 (LPDDR2)
@@ -213,37 +365,35 @@ wait_pll1_lock:
 //   DDR controller setting
 //*===========================================
 // CTL setting
-    ldr r0, =DATABAHN_BASE_ADDR
-
-//*==========================================
-// Load ZQ
-//*==========================================
-//setmem /32 0x14000128 = 0x00170101  // load PU
-    ldr r1, =0x00170101
-    str r1, [r0, #0x128]
-
-    ldr r1, =0x1000
-delay0:
-    sub r1, r1, #0x1
-    cmp r1, #0x0
-    bne delay0
-
-//setmem /32 0x14000128 = 0x00000000  // clear
-    ldr r1, =0x00000000
-    str r1, [r0, #0x128]
-//setmem /32 0x14000128 = 0x08170111  // load PD
-    ldr r1, =0x08170111
-    str r1, [r0, #0x128]
-
-    ldr r1, =0x1000
-delay1:
-    sub r1, r1, #0x1
-    cmp r1, #0x0
-    bne delay1
-
-//setmem /32 0x14000128 = 0x00000000  // clear
-    ldr r1, =0x00000000
-    str r1, [r0, #0x128]
+	ldr r0, =DATABAHN_BASE_ADDR
+
+#if defined(CONFIG_ZQ_CALIB)
+	do_zq_calib
+#else
+// setmem /32 0x1400012C = 0x00000817  // pd=<<8, pu=<<0
+	ldr r1, =0x00000817
+	str r1, [r0, #0x12c]
+// setmem /32 0x14000128 = 0x09180000  // (pd+1)<<24, (pu+1)<<16
+	ldr r1, =0x09180000
+	str r1, [r0, #0x128]
+// load PU, pu_pd_sel=0
+// setmem /32 0x14000124 = 0x00310000  // software load ZQ: 3<<20, 1<<16
+	ldr r1, =0x00310000
+	str r1, [r0, #0x124]
+// setmem /32 0x14000124 = 0x00200000  // clear for next load
+	ldr r1, =0x00200000
+	str r1, [r0, #0x124]
+// load PD, pu_pd_sel=1
+// setmem /32 0x14000128 = 0x09180010  // (pd+1)<<24, (pu+1)<<16, 1<<4
+	ldr r1, =0x09180010
+	str r1, [r0, #0x128]
+// setmem /32 0x14000124 = 0x00310000  // software load ZQ: 3<<20, 1<<16
+	ldr r1, =0x00310000
+	str r1, [r0, #0x124]
+// setmem /32 0x14000124 = 0x00200000  // clear for next load
+	ldr r1, =0x00200000
+	str r1, [r0, #0x124]
+#endif
 
 //setmem /32 0x14000000 = 0x00000500
     ldr r1, =0x00000500
@@ -450,8 +600,8 @@ delay1:
     ldr r1, =0x01010002
     str r1, [r0, #0x118]
 
-//setmem /32 0x1400011c = 0x00000000
-    ldr r1, =0x00000000
+//setmem /32 0x1400011c = 0x00001000
+    ldr r1, =0x00001000
     str r1, [r0, #0x11c]
 
 // PHY setting
@@ -594,14 +744,15 @@ before_calling_rom___pu_irom_hwcnfg_setup:
 	blxhi r3
 after_calling_rom___pu_irom_hwcnfg_setup:
 
-	pop {r0-r4, lr}
+	pop {r0-r6, r8, lr}
 	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
+DDR_DEST_ADDR:		.word TEXT_BASE
+COPY_SIZE:		.word _end - TEXT_BASE
+BOOT_DATA:		.word TEXT_BASE
+			.word _end - TEXT_BASE
+			.word 0
+IVT2_HDR_OFS:		.word ivt2_header - TEXT_BASE
 
 #endif