diff --git a/plat/intel/soc/common/aarch64/plat_helpers.S b/plat/intel/soc/common/aarch64/plat_helpers.S
index 9a17587..b3d5665 100644
--- a/plat/intel/soc/common/aarch64/plat_helpers.S
+++ b/plat/intel/soc/common/aarch64/plat_helpers.S
@@ -98,18 +98,6 @@
 endfunc plat_my_core_pos
 
 func warm_reset_req
-#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
-	bl	plat_is_my_cpu_primary
-	cbnz	x0, warm_reset
-warm_reset:
-	mov_imm x1, PLAT_SEC_ENTRY
-	str	xzr, [x1]
-	mrs	x1, rmr_el3
-	orr	x1, x1, #0x02
-	msr	rmr_el3, x1
-	isb
-	dsb	sy
-#else
 	str	xzr, [x4]
 	bl	plat_is_my_cpu_primary
 	cbz	x0, cpu_in_wfi
@@ -123,22 +111,35 @@
 cpu_in_wfi:
 	wfi
 	b	cpu_in_wfi
-#endif
 endfunc warm_reset_req
 
-/* TODO: Zephyr warm reset test */
 #if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
 func plat_get_my_entrypoint
 	ldr	x4, =L2_RESET_DONE_REG
 	ldr	x5, [x4]
-	ldr	x1, =PLAT_L2_RESET_REQ
+
+	/* Check for warm reset request */
+	ldr	x1, =L2_RESET_DONE_STATUS
 	cmp	x1, x5
-	b.eq	zephyr_reset_req
+	b.eq	warm_reset_req
+
+	/* Check for SMP secondary cores boot request */
+	ldr	x1, =SMP_SEC_CORE_BOOT_REQ
+	cmp	x1, x5
+	b.eq	smp_request
+
+	/* Otherwise it is cold reset */
+	mov	x0, #0
+	ret
+smp_request:
+	/*
+	 * Return the address 'bl31_warm_entrypoint', which is passed to
+	 * 'psci_setup' routine as part of BL31 initialization.
+	 */
 	mov_imm	x1, PLAT_SEC_ENTRY
 	ldr	x0, [x1]
-	ret
-zephyr_reset_req:
-	ldr	x0, =0x00
+	/* Clear the mark up before return */
+	str	xzr, [x4]
 	ret
 endfunc plat_get_my_entrypoint
 #else
diff --git a/plat/intel/soc/common/include/platform_def.h b/plat/intel/soc/common/include/platform_def.h
index b388456..08ab5df 100644
--- a/plat/intel/soc/common/include/platform_def.h
+++ b/plat/intel/soc/common/include/platform_def.h
@@ -29,6 +29,9 @@
 /* Magic word to indicate L2 reset is completed */
 #define L2_RESET_DONE_STATUS			0x1228E5E7
 
+/* Magic word to differentiate for SMP secondary core boot request */
+#define SMP_SEC_CORE_BOOT_REQ			0x1228E5E8
+
 /* Define next boot image name and offset */
 /* Get non-secure image entrypoint for BL33. Zephyr and Linux */
 #ifdef PRELOADED_BL33_BASE
diff --git a/plat/intel/soc/common/socfpga_psci.c b/plat/intel/soc/common/socfpga_psci.c
index 8dc39e2..50d4820 100644
--- a/plat/intel/soc/common/socfpga_psci.c
+++ b/plat/intel/soc/common/socfpga_psci.c
@@ -213,6 +213,16 @@
 static int socfpga_system_reset2(int is_vendor, int reset_type,
 					u_register_t cookie)
 {
+
+#if CACHE_FLUSH
+	/*
+	 * ATF Flush and Invalidate Cache due to hardware limitation
+	 * of auto Flush and Invalidate Cache.
+	 */
+	dcsw_op_all(DCCISW);
+	invalidate_cache_low_el();
+#endif
+
 #if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
 	mailbox_reset_warm(reset_type);
 #else