diff --git a/arch/arm/mach-socfpga/include/mach/system_manager_soc64.h b/arch/arm/mach-socfpga/include/mach/system_manager_soc64.h
index 0871cf949e5..713fa2c517b 100644
--- a/arch/arm/mach-socfpga/include/mach/system_manager_soc64.h
+++ b/arch/arm/mach-socfpga/include/mach/system_manager_soc64.h
@@ -148,6 +148,8 @@ void populate_sysmgr_pinmux(void);
  * Bit[30] reserved for FSBL to update the DDR init progress
  * 1 - means in progress, 0 - haven't started / DDR is up running.
  *
+ * Bit[19] store ATF CPU0 ON OFF value.
+ *
  * Bit[18] reserved for SDM to configure ACF
  * Bit[17:1] - Setting by Linux EDAC.
  * Bit[1](ECC_OCRAM), Bit[16](ECC_DDR0), Bit[17](ECC_DDR1)
diff --git a/arch/arm/mach-socfpga/lowlevel_init_soc64.S b/arch/arm/mach-socfpga/lowlevel_init_soc64.S
index 8926c2d1d9c..b39565f591d 100644
--- a/arch/arm/mach-socfpga/lowlevel_init_soc64.S
+++ b/arch/arm/mach-socfpga/lowlevel_init_soc64.S
@@ -12,15 +12,110 @@
 ENTRY(lowlevel_init)
 	mov	x29, lr			/* Save LR */
 
+#ifdef CONFIG_XPL_BUILD
+	/* Check for L2 reset magic word */
+	ldr	x4, =L2_RESET_DONE_REG
+	ldr	x5, [x4]
+	ldr	x1, =L2_RESET_DONE_STATUS
+	cmp	x1, x5
+	/* No L2 reset, skip warm reset */
+	b.ne	skipwarmreset
+	/* Put all slaves CPUs into WFI mode */
+	branch_if_slave x0, put_cpu_in_wfi
+	/* L2 reset completed */
+	str	xzr, [x4]
+	/* Clear previous CPU release address */
+	ldr	x4, =CPU_RELEASE_ADDR
+	str	wzr, [x4]
+	/* Master CPU (CPU0) request for warm reset */
+	mrs	x1, rmr_el3
+	orr	x1, x1, #0x02
+	msr	rmr_el3, x1
+	isb
+	dsb	sy
+put_cpu_in_wfi:
+	wfi
+	b	put_cpu_in_wfi
+skipwarmreset:
+#endif
+
 #if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
 #if defined(CONFIG_XPL_BUILD) && defined(CONFIG_SPL_ATF)
+
+	/*
+	 * In ATF flow, need to clear the old CPU address when cold reset
+	 * being triggered, but shouldn't clear CPU address if it is reset
+	 * by CPU-ON, so that the core can correctly jump to ATF code after
+	 * reset by CPU-ON. CPU-ON trigger the reset via mpumodrst.
+	 *
+	 * Hardware will set 1 to core*_irq in mpurststat register in
+	 * reset manager if the core is reset by mpumodrst.
+	 *
+	 * The following code will check the mpurststat to identify if the
+	 * core is reset by mpumodrst, and it will skip CPU address clearing
+	 * if the core is reset by mpumodrst. At last, the code need to clear
+	 * the core*_irq by set it to 1. So that it can reflect the correct
+	 * and latest status in next reset.
+	 */
+
+	/* Check if it is a master core off/on from kernel using boot scratch
+	 * cold register 8 bit 19. This bit is set by ATF.
+	 */
+	ldr	x4, =BOOT_SCRATCH_COLD8
+	ldr	x5, [x4]
+	and	x6, x5, #0x80000
+	cbnz	x6, wait_for_atf_master
+
+	/* Retrieve mpurststat register in reset manager */
+	ldr	x4, =SOCFPGA_RSTMGR_ADDRESS
+	ldr	w5, [x4, #0x04]
+
+	/* Set mask based on current core id */
+	mrs	x0, mpidr_el1
+	and	x1, x0, #0xF
+	ldr	x2, =0x00000100
+	lsl	x2, x2, x1
+
+	/* Skip if core*_irq register is set */
+	and	x6, x5, x2
+	cbnz	x6, skip_clear_cpu_address
+
+	/*
+	 * Reach here means core*_irq is 0, means the core is
+	 * reset by cold, warm or watchdog reset.
+	 * Clear previous CPU release address
+	 */
+	ldr	x4, =CPU_RELEASE_ADDR
+	str	wzr, [x4]
+	b	skip_clear_core_irq
+
+skip_clear_cpu_address:
+	/* Clear core*_irq register by writing 1 */
+	ldr	x4, =SOCFPGA_RSTMGR_ADDRESS
+	str	w2, [x4, #0x04]
+
+skip_clear_core_irq:
+	/* Master CPU (CPU0) does not need to wait for atf */
+	branch_if_master x0, master_cpu
+
 wait_for_atf:
 	ldr	x4, =CPU_RELEASE_ADDR
 	ldr	x5, [x4]
 	cbz	x5, slave_wait_atf
 	br	x5
+
 slave_wait_atf:
 	branch_if_slave x0, wait_for_atf
+
+wait_for_atf_master:
+	ldr	x4, =CPU_RELEASE_ADDR
+	ldr	x5, [x4]
+	cbz	x5, master_wait_atf
+	br	x5
+master_wait_atf:
+	branch_if_master x0, wait_for_atf_master
+
+master_cpu:
 #else
 	branch_if_slave x0, 1f
 #endif
diff --git a/include/configs/socfpga_soc64_common.h b/include/configs/socfpga_soc64_common.h
index 8755532ea82..3d09a06f63e 100644
--- a/include/configs/socfpga_soc64_common.h
+++ b/include/configs/socfpga_soc64_common.h
@@ -24,6 +24,11 @@
  */
 #define L2_RESET_DONE_REG		0xFFD12218
 
+/* sysmgr.boot_scratch_cold8 bit 17 (1bit) will be used to check whether CPU0
+ * is being powered off/on from kernel
+ */
+#define BOOT_SCRATCH_COLD8		0xFFD12220
+
 /* Magic word to indicate L2 reset is completed */
 #define L2_RESET_DONE_STATUS		0x1228E5E7