feat(intel): reset manager support for Agilex5 SoC FPGA

This patch is used to enable reset manager support
for Agilex5 SoC FPGA.
	1. Added HPS bridges support
		a. SOC2FPGA
		b. LWSOC2FPGA
		c. F2SDRAM
		d. F2SOC
	2. Added EMULATOR support
	3. Added WDT support
	4. Updated product name -> Agilex5
	5. Added SMP support

Signed-off-by: Jit Loon Lim <jit.loon.lim@intel.com>
Change-Id: Icab15b25f787fdccce1de75d102604db23beaf11
diff --git a/plat/intel/soc/common/include/socfpga_f2sdram_manager.h b/plat/intel/soc/common/include/socfpga_f2sdram_manager.h
index b30a11e..1bebfc9 100644
--- a/plat/intel/soc/common/include/socfpga_f2sdram_manager.h
+++ b/plat/intel/soc/common/include/socfpga_f2sdram_manager.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,6 +14,7 @@
 #define SOCFPGA_F2SDRAMMGR_SIDEBANDMGR_FLAGOUTCLR0	0x54
 #define SOCFPGA_F2SDRAMMGR_SIDEBANDMGR_FLAGOUTSET0	0x50
 
+#define FLAGOUTCLR0_F2SDRAM0_ENABLE		(BIT(8))
 #define FLAGOUTSETCLR_F2SDRAM0_ENABLE		(BIT(1))
 #define FLAGOUTSETCLR_F2SDRAM1_ENABLE		(BIT(4))
 #define FLAGOUTSETCLR_F2SDRAM2_ENABLE		(BIT(7))
diff --git a/plat/intel/soc/common/include/socfpga_reset_manager.h b/plat/intel/soc/common/include/socfpga_reset_manager.h
index cce16ab..9d06a3d 100644
--- a/plat/intel/soc/common/include/socfpga_reset_manager.h
+++ b/plat/intel/soc/common/include/socfpga_reset_manager.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2022, Intel Corporation. All rights reserved.
+ * Copyright (c) 2019-2023, Intel Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,10 @@
 
 #include "socfpga_plat_def.h"
 
+/* Status Response */
+#define RSTMGR_RET_OK				0
+#define RSTMGR_RET_ERROR			-1
+
 #define SOCFPGA_BRIDGE_ENABLE			BIT(0)
 #define SOCFPGA_BRIDGE_HAS_MASK			BIT(1)
 
@@ -22,28 +26,51 @@
 /* Register Mapping */
 
 #define SOCFPGA_RSTMGR_STAT			0x000
+#define SOCFPGA_RSTMGR_MISCSTAT			0x008
 #define SOCFPGA_RSTMGR_HDSKEN			0x010
 #define SOCFPGA_RSTMGR_HDSKREQ			0x014
 #define SOCFPGA_RSTMGR_HDSKACK			0x018
+#define SOCFPGA_RSTMGR_HDSKSTALL		0x01C
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 #define SOCFPGA_RSTMGR_MPUMODRST		0x020
+#endif
 #define SOCFPGA_RSTMGR_PER0MODRST		0x024
 #define SOCFPGA_RSTMGR_PER1MODRST		0x028
-#define SOCFPGA_RSTMGR_BRGMODRST		0x02c
+#define SOCFPGA_RSTMGR_BRGMODRST		0x02C
+#if PLATFORM_MODEL != PLAT_SOCFPGA_AGILEX5
 #define SOCFPGA_RSTMGR_COLDMODRST		0x034
+#endif
+#define SOCFPGA_RSTMGR_DBGMODRST		0x03C
+#define SOCFPGA_RSTMGR_BRGWARMMASK		0x04C
+#define SOCFPGA_RSTMGR_TSTSTA			0x05C
 #define SOCFPGA_RSTMGR_HDSKTIMEOUT		0x064
+#define SOCFPGA_RSTMGR_DBGHDSKTIMEOUT		0x06C
+#define SOCFPGA_RSTMGR_DBGRSTCMPLT		0x070
+#define SOCFPGA_RSTMGR_HPSRSTCMPLT		0x080
+#define SOCFPGA_RSTMGR_CPUINREST		0x090
+#define SOCFPGA_RSTMGR_CPURSTRELEASE		0x094
+#define SOCFPGA_RSTMGR_CPUBASELOW_0		0x098
+#define SOCFPGA_RSTMGR_CPUBASEHIGH_0		0x09C
+#define SOCFPGA_RSTMGR_CPUBASELOW_1		0x0A0
+#define SOCFPGA_RSTMGR_CPUBASEHIGH_1		0x0A4
+#define SOCFPGA_RSTMGR_CPUBASELOW_2		0x0A8
+#define SOCFPGA_RSTMGR_CPUBASEHIGH_2		0x0AC
+#define SOCFPGA_RSTMGR_CPUBASELOW_3		0x0B0
+#define SOCFPGA_RSTMGR_CPUBASEHIGH_3		0x0B4
 
 /* Field Mapping */
-
-#define RSTMGR_PER0MODRST_EMAC0			0x00000001
-#define RSTMGR_PER0MODRST_EMAC1			0x00000002
-#define RSTMGR_PER0MODRST_EMAC2			0x00000004
+/* PER0MODRST */
+#define RSTMGR_PER0MODRST_EMAC0			0x00000001	//TSN0
+#define RSTMGR_PER0MODRST_EMAC1			0x00000002	//TSN1
+#define RSTMGR_PER0MODRST_EMAC2			0x00000004	//TSN2
 #define RSTMGR_PER0MODRST_USB0			0x00000008
 #define RSTMGR_PER0MODRST_USB1			0x00000010
 #define RSTMGR_PER0MODRST_NAND			0x00000020
+#define RSTMGR_PER0MODRST_SOFTPHY		0x00000040
 #define RSTMGR_PER0MODRST_SDMMC			0x00000080
-#define RSTMGR_PER0MODRST_EMAC0OCP		0x00000100
-#define RSTMGR_PER0MODRST_EMAC1OCP		0x00000200
-#define RSTMGR_PER0MODRST_EMAC2OCP		0x00000400
+#define RSTMGR_PER0MODRST_EMAC0OCP		0x00000100	//TSN0ECC
+#define RSTMGR_PER0MODRST_EMAC1OCP		0x00000200	//TSN1ECC
+#define RSTMGR_PER0MODRST_EMAC2OCP		0x00000400	//TSN2ECC
 #define RSTMGR_PER0MODRST_USB0OCP		0x00000800
 #define RSTMGR_PER0MODRST_USB1OCP		0x00001000
 #define RSTMGR_PER0MODRST_NANDOCP		0x00002000
@@ -64,6 +91,7 @@
 #define RSTMGR_PER0MODRST_DMAIF6		0x40000000
 #define RSTMGR_PER0MODRST_DMAIF7		0x80000000
 
+/* PER1MODRST */
 #define RSTMGR_PER1MODRST_WATCHDOG0		0x00000001
 #define RSTMGR_PER1MODRST_WATCHDOG1		0x00000002
 #define RSTMGR_PER1MODRST_WATCHDOG2		0x00000004
@@ -77,31 +105,121 @@
 #define RSTMGR_PER1MODRST_I2C2			0x00000400
 #define RSTMGR_PER1MODRST_I2C3			0x00000800
 #define RSTMGR_PER1MODRST_I2C4			0x00001000
+#define RSTMGR_PER1MODRST_I3C0			0x00002000
+#define RSTMGR_PER1MODRST_I3C1			0x00004000
 #define RSTMGR_PER1MODRST_UART0			0x00010000
 #define RSTMGR_PER1MODRST_UART1			0x00020000
 #define RSTMGR_PER1MODRST_GPIO0			0x01000000
 #define RSTMGR_PER1MODRST_GPIO1			0x02000000
+#define RSTMGR_PER1MODRST_WATCHDOG4		0x04000000
 
+/* HDSKEN */
+#define RSTMGR_HDSKEN_EMIF_FLUSH		0x00000001
 #define RSTMGR_HDSKEN_FPGAHSEN			0x00000004
 #define RSTMGR_HDSKEN_ETRSTALLEN		0x00000008
-#define RSTMGR_HDSKEN_L2FLUSHEN			0x00000100
+#define RSTMGR_HDSKEN_LWS2F_FLUSH		0x00000200
+#define RSTMGR_HDSKEN_S2F_FLUSH			0x00000400
+#define RSTMGR_HDSKEN_F2SDRAM_FLUSH		0x00000800
+#define RSTMGR_HDSKEN_F2S_FLUSH			0x00001000
 #define RSTMGR_HDSKEN_L3NOC_DBG			0x00010000
 #define RSTMGR_HDSKEN_DEBUG_L3NOC		0x00020000
-#define RSTMGR_HDSKEN_SDRSELFREFEN		0x00000001
 
-#define RSTMGR_HDSKEQ_FPGAHSREQ			0x4
+/* HDSKREQ */
+#define RSTMGR_HDSKREQ_EMIFFLUSHREQ		0x00000001
+#define RSTMGR_HDSKREQ_ETRSTALLREQ		0x00000008
+#define RSTMGR_HDSKREQ_LWS2F_FLUSH		0x00000200
+#define RSTMGR_HDSKREQ_S2F_FLUSH		0x00000400
+#define RSTMGR_HDSKREQ_F2SDRAM_FLUSH		0x00000800
+#define RSTMGR_HDSKREQ_F2S_FLUSH		0x00001000
+#define RSTMGR_HDSKREQ_L3NOC_DBG		0x00010000
+#define RSTMGR_HDSKREQ_DEBUG_L3NOC		0x00020000
+#define RSTMGR_HDSKREQ_FPGAHSREQ		0x00000004
+#define RSTMGR_HDSKREQ_LWSOC2FPGAREQ		0x00000200
+#define RSTMGR_HDSKREQ_SOC2FPGAREQ		0x00000400
+#define RSTMGR_HDSKREQ_F2SDRAM0REQ		0x00000800
+#define RSTMGR_HDSKREQ_FPGA2SOCREQ		0x00001000
 
-#define RSTMGR_BRGMODRST_SOC2FPGA		0x1
-#define RSTMGR_BRGMODRST_LWHPS2FPGA		0x2
-#define RSTMGR_BRGMODRST_FPGA2SOC		0x4
-#define RSTMGR_BRGMODRST_F2SSDRAM0		0x8
+/* HDSKACK */
+#define RSTMGR_HDSKACK_EMIFFLUSHREQ		0x00000001
+#define RSTMGR_HDSKACK_FPGAHSREQ		0x00000004
+#define RSTMGR_HDSKACK_ETRSTALLREQ		0x00000008
+#define RSTMGR_HDSKACK_LWS2F_FLUSH		0x00000200
+#define RSTMGR_HDSKACK_S2F_FLUSH		0x00000400
+#define RSTMGR_HDSKACK_F2SDRAM_FLUSH		0x00000800
+#define RSTMGR_HDSKACK_F2S_FLUSH		0x00001000
+#define RSTMGR_HDSKACK_L3NOC_DBG		0x00010000
+#define RSTMGR_HDSKACK_DEBUG_L3NOC		0x00020000
+#define RSTMGR_HDSKACK_FPGAHSACK		0x00000004
+#define RSTMGR_HDSKACK_LWSOC2FPGAACK		0x00000200
+#define RSTMGR_HDSKACK_SOC2FPGAACK		0x00000400
+#define RSTMGR_HDSKACK_F2SDRAM0ACK		0x00000800
+#define RSTMGR_HDSKACK_FPGA2SOCACK		0x00001000
+#define RSTMGR_HDSKACK_FPGAHSACK_DASRT		0x00000000
+#define RSTMGR_HDSKACK_F2SDRAM0ACK_DASRT	0x00000000
+#define RSTMGR_HDSKACK_FPGA2SOCACK_DASRT	0x00000000
+
+/* HDSKSTALL */
+#define RSTMGR_HDSKACK_ETRSTALLWARMRST		0x00000001
+
+/* BRGMODRST */
+#define RSTMGR_BRGMODRST_SOC2FPGA		0x00000001
+#define RSTMGR_BRGMODRST_LWHPS2FPGA		0x00000002
+#define RSTMGR_BRGMODRST_FPGA2SOC		0x00000004
+#define RSTMGR_BRGMODRST_F2SSDRAM0		0x00000008
+#if PLATFORM_MODEL == PLAT_SOCFPGA_STRATIX10
 #define RSTMGR_BRGMODRST_F2SSDRAM1		0x10
 #define RSTMGR_BRGMODRST_F2SSDRAM2		0x20
-#define RSTMGR_BRGMODRST_MPFE			0x40
 #define RSTMGR_BRGMODRST_DDRSCH			0x40
+#elif PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+#define RSTMGR_BRGMODRST_F2SSDRAM1		0x10
+#define RSTMGR_BRGMODRST_F2SSDRAM2		0x20
+#endif
+
+#define RSTMGR_BRGMODRST_MPFE			0x40
+
+/* DBGMODRST */
+#define RSTMGR_DBGMODRST_DBG_RST		0x00000001
 
-#define RSTMGR_HDSKREQ_FPGAHSREQ		(BIT(2))
-#define RSTMGR_HDSKACK_FPGAHSACK_MASK		(BIT(2))
+/* BRGMODRSTMASK */
+#define RSTMGR_BRGMODRSTMASK_SOC2FPGA		0x00000001
+#define RSTMGR_BRGMODRSTMASK_LWHPS2FPGA		0x00000002
+#define RSTMGR_BRGMODRSTMASK_FPGA2SOC		0x00000004
+#define RSTMGR_BRGMODRSTMASK_F2SDRAM0		0x00000008
+#define RSTMGR_BRGMODRSTMASK_MPFE		0x00000040
+
+/* TSTSTA */
+#define RSTMGR_TSTSTA_RSTST			0x0000001F
+
+/* HDSKTIMEOUT */
+#define RSTMGR_HDSKTIMEOUT_VAL			0xFFFFFFFF
+
+/* DBGHDSKTIMEOUT */
+#define RSTMGR_DBGHDSKTIMEOUT_VAL		0xFFFFFFFF
+
+/* DBGRSTCMPLT */
+#define RSTMGR_DBGRSTCMPLT_VAL			0xFFFFFFFF
+
+/* HPSRSTCMPLT */
+#define RSTMGR_DBGRSTCMPLT_VAL			0xFFFFFFFF
+
+/* CPUINRESET */
+#define RSTMGR_CPUINRESET_CPU0			0x00000001
+#define RSTMGR_CPUINRESET_CPU1			0x00000002
+#define RSTMGR_CPUINRESET_CPU2			0x00000004
+#define RSTMGR_CPUINRESET_CPU3			0x00000008
+
+/* CPUSTRELEASE */
+#define RSTMGR_CPUSTRELEASE_CPUx		0x10D11094
+
+/* CPUxRESETBASE */
+#define RSTMGR_CPUxRESETBASELOW_CPU0		0x10D11098
+#define RSTMGR_CPUxRESETBASEHIGH_CPU0		0x10D1109C
+#define RSTMGR_CPUxRESETBASELOW_CPU1		0x10D110A0
+#define RSTMGR_CPUxRESETBASEHIGH_CPU1		0x10D110A4
+#define RSTMGR_CPUxRESETBASELOW_CPU2		0x10D110A8
+#define RSTMGR_CPUxRESETBASEHIGH_CPU2		0x10D110AC
+#define RSTMGR_CPUxRESETBASELOW_CPU3		0x10D110B0
+#define RSTMGR_CPUxRESETBASEHIGH_CPU3		0x10D110B4
 
 /* Definitions */
 
@@ -109,17 +227,28 @@
 #define RSTMGR_HDSKEN_SET			0x010D
 
 /* Macros */
+#define SOCFPGA_RSTMGR(_reg)			(SOCFPGA_RSTMGR_REG_BASE + (SOCFPGA_RSTMGR_##_reg))
+#define RSTMGR_FIELD(_reg, _field)		(RSTMGR_##_reg##MODRST_##_field)
 
-#define SOCFPGA_RSTMGR(_reg)		(SOCFPGA_RSTMGR_REG_BASE \
-					+ (SOCFPGA_RSTMGR_##_reg))
-#define RSTMGR_FIELD(_reg, _field)	(RSTMGR_##_reg##MODRST_##_field)
+/* Reset type to SDM from PSCI */
+// Temp add macro here for reset type
+#define SOCFPGA_RESET_TYPE_COLD			0
+#define SOCFPGA_RESET_TYPE_WARM			1
 
 /* Function Declarations */
 
 void deassert_peripheral_reset(void);
 void config_hps_hs_before_warm_reset(void);
 
+int socfpga_bridges_reset(uint32_t mask);
 int socfpga_bridges_enable(uint32_t mask);
 int socfpga_bridges_disable(uint32_t mask);
 
+int socfpga_cpurstrelease(unsigned int cpu_id);
+int socfpga_cpu_reset_base(unsigned int cpu_id);
+
+/* SMP: Func proto */
+void bl31_plat_set_secondary_cpu_entrypoint(unsigned int cpu_id);
+void bl31_plat_set_secondary_cpu_off(void);
+
 #endif /* SOCFPGA_RESETMANAGER_H */
diff --git a/plat/intel/soc/common/soc/socfpga_reset_manager.c b/plat/intel/soc/common/soc/socfpga_reset_manager.c
index a546638..7db86c7 100644
--- a/plat/intel/soc/common/soc/socfpga_reset_manager.c
+++ b/plat/intel/soc/common/soc/socfpga_reset_manager.c
@@ -4,6 +4,7 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
 #include <errno.h>
 #include <common/debug.h>
 #include <drivers/delay_timer.h>
@@ -23,6 +24,7 @@
 			RSTMGR_FIELD(PER1, WATCHDOG1) |
 			RSTMGR_FIELD(PER1, WATCHDOG2) |
 			RSTMGR_FIELD(PER1, WATCHDOG3) |
+			RSTMGR_FIELD(PER1, WATCHDOG4) |
 			RSTMGR_FIELD(PER1, L4SYSTIMER0) |
 			RSTMGR_FIELD(PER1, L4SYSTIMER1) |
 			RSTMGR_FIELD(PER1, SPTIMER0) |
@@ -32,12 +34,15 @@
 			RSTMGR_FIELD(PER1, I2C2) |
 			RSTMGR_FIELD(PER1, I2C3) |
 			RSTMGR_FIELD(PER1, I2C4) |
+			RSTMGR_FIELD(PER1, I3C0) |
+			RSTMGR_FIELD(PER1, I3C1) |
 			RSTMGR_FIELD(PER1, UART0) |
 			RSTMGR_FIELD(PER1, UART1) |
 			RSTMGR_FIELD(PER1, GPIO0) |
 			RSTMGR_FIELD(PER1, GPIO1));
 
 	mmio_clrbits_32(SOCFPGA_RSTMGR(PER0MODRST),
+			RSTMGR_FIELD(PER0, SOFTPHY) |
 			RSTMGR_FIELD(PER0, EMAC0OCP) |
 			RSTMGR_FIELD(PER0, EMAC1OCP) |
 			RSTMGR_FIELD(PER0, EMAC2OCP) |
@@ -80,10 +85,10 @@
 {
 	uint32_t or_mask = 0;
 
-	or_mask |= RSTMGR_HDSKEN_SDRSELFREFEN;
+	or_mask |= RSTMGR_HDSKEN_EMIF_FLUSH;
 	or_mask |= RSTMGR_HDSKEN_FPGAHSEN;
 	or_mask |= RSTMGR_HDSKEN_ETRSTALLEN;
-	or_mask |= RSTMGR_HDSKEN_L2FLUSHEN;
+	or_mask |= RSTMGR_HDSKEN_LWS2F_FLUSH;
 	or_mask |= RSTMGR_HDSKEN_L3NOC_DBG;
 	or_mask |= RSTMGR_HDSKEN_DEBUG_L3NOC;
 
@@ -104,6 +109,27 @@
 	return -ETIMEDOUT;
 }
 
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+static int poll_idle_status_by_counter(uint32_t addr, uint32_t mask,
+					uint32_t match, uint32_t delay_ms)
+{
+	int time_out = delay_ms;
+
+	while (time_out-- > 0) {
+
+		if ((mmio_read_32(addr) & mask) == match) {
+			return 0;
+		}
+
+		/* ToDo: Shall use udelay for product release */
+		for (int i = 0; i < 2000; i++) {
+			/* dummy delay */
+		}
+	}
+	return -ETIMEDOUT;
+}
+#endif
+
 static int poll_idle_status_by_clkcycles(uint32_t addr, uint32_t mask,
 					 uint32_t match, uint32_t delay_clk_cycles)
 {
@@ -185,6 +211,39 @@
 		*f2s_respempty |= FLAGINSTATUS_F2SDRAM2_RESPEMPTY;
 		*f2s_cmdidle |= FLAGINSTATUS_F2SDRAM2_CMDIDLE;
 	}
+#elif PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	if (mask & FPGA2SOC_MASK) {
+		*brg_mask |= RSTMGR_FIELD(BRG, FPGA2SOC);
+		*f2s_idlereq |= FLAGOUTSETCLR_F2SDRAM0_IDLEREQ;
+		*f2s_force_drain |= FLAGOUTSETCLR_F2SDRAM0_FORCE_DRAIN;
+		*f2s_en |= FLAGOUTSETCLR_F2SDRAM0_ENABLE;
+		*f2s_idleack |= FLAGINSTATUS_F2SDRAM0_IDLEACK;
+		*f2s_respempty |= FLAGINSTATUS_F2SDRAM0_RESPEMPTY;
+	}
+	if (mask & F2SDRAM0_MASK) {
+		*brg_mask |= RSTMGR_FIELD(BRG, F2SSDRAM0);
+		*f2s_idlereq |= FLAGOUTSETCLR_F2SDRAM0_IDLEREQ;
+		*f2s_force_drain |= FLAGOUTSETCLR_F2SDRAM0_FORCE_DRAIN;
+		*f2s_en |= FLAGOUTSETCLR_F2SDRAM0_ENABLE;
+		*f2s_idleack |= FLAGINSTATUS_F2SDRAM0_IDLEACK;
+		*f2s_respempty |= FLAGINSTATUS_F2SDRAM0_RESPEMPTY;
+	}
+	if (mask & F2SDRAM1_MASK) {
+		*brg_mask |= RSTMGR_FIELD(BRG, F2SSDRAM1);
+		*f2s_idlereq |= FLAGOUTSETCLR_F2SDRAM1_IDLEREQ;
+		*f2s_force_drain |= FLAGOUTSETCLR_F2SDRAM1_FORCE_DRAIN;
+		*f2s_en |= FLAGOUTSETCLR_F2SDRAM1_ENABLE;
+		*f2s_idleack |= FLAGINSTATUS_F2SDRAM1_IDLEACK;
+		*f2s_respempty |= FLAGINSTATUS_F2SDRAM1_RESPEMPTY;
+	}
+	if (mask & F2SDRAM2_MASK) {
+		*brg_mask |= RSTMGR_FIELD(BRG, F2SSDRAM2);
+		*f2s_idlereq |= FLAGOUTSETCLR_F2SDRAM2_IDLEREQ;
+		*f2s_force_drain |= FLAGOUTSETCLR_F2SDRAM2_FORCE_DRAIN;
+		*f2s_en |= FLAGOUTSETCLR_F2SDRAM2_ENABLE;
+		*f2s_idleack |= FLAGINSTATUS_F2SDRAM2_IDLEACK;
+		*f2s_respempty |= FLAGINSTATUS_F2SDRAM2_RESPEMPTY;
+	}
 #else
 	if ((mask & FPGA2SOC_MASK) != 0U) {
 		*brg_mask |= RSTMGR_FIELD(BRG, FPGA2SOC);
@@ -198,6 +257,153 @@
 #endif
 }
 
+int socfpga_bridges_reset(uint32_t mask)
+{
+	int ret = 0;
+	int timeout = 300;
+	uint32_t brg_mask = 0;
+	uint32_t noc_mask = 0;
+	uint32_t f2s_idlereq = 0;
+	uint32_t f2s_force_drain = 0;
+	uint32_t f2s_en = 0;
+	uint32_t f2s_idleack = 0;
+	uint32_t f2s_respempty = 0;
+	uint32_t f2s_cmdidle = 0;
+
+	/* Reset s2f bridge */
+	socfpga_s2f_bridge_mask(mask, &brg_mask, &noc_mask);
+	if (brg_mask) {
+		if (mask & SOC2FPGA_MASK) {
+			/* Request handshake with SOC2FPGA bridge to clear traffic */
+			mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+					RSTMGR_HDSKREQ_S2F_FLUSH);
+
+			/* Wait for bridge to idle status */
+			ret = poll_idle_status(SOCFPGA_RSTMGR(HDSKACK),
+					RSTMGR_HDSKACK_S2F_FLUSH,
+					RSTMGR_HDSKACK_S2F_FLUSH, 300);
+		}
+
+		if (mask & LWHPS2FPGA_MASK) {
+			/* Request handshake with LWSOC2FPGA bridge to clear traffic */
+			mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+					RSTMGR_HDSKREQ_LWS2F_FLUSH);
+
+			/* Wait for bridge to idle status */
+			ret = poll_idle_status(SOCFPGA_RSTMGR(HDSKACK),
+					RSTMGR_HDSKACK_LWS2F_FLUSH,
+					RSTMGR_HDSKACK_LWS2F_FLUSH, 300);
+		}
+
+		if (ret < 0) {
+			ERROR("S2F Bridge reset: Timeout waiting for idle ack\n");
+			assert(false);
+		}
+
+		/* Assert reset to bridge */
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+				brg_mask);
+
+		/* Clear idle requests to bridge */
+		if (mask & SOC2FPGA_MASK) {
+			mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+					RSTMGR_HDSKREQ_S2F_FLUSH);
+		}
+
+		if (mask & LWHPS2FPGA_MASK) {
+			mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+					RSTMGR_HDSKREQ_LWS2F_FLUSH);
+		}
+
+		/* When FPGA reconfig is complete */
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
+	}
+
+	/* Reset f2s bridge */
+	socfpga_f2s_bridge_mask(mask, &brg_mask, &f2s_idlereq,
+					&f2s_force_drain, &f2s_en,
+					&f2s_idleack, &f2s_respempty,
+					&f2s_cmdidle);
+
+	if (brg_mask) {
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKEN),
+				RSTMGR_HDSKEN_FPGAHSEN);
+
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+				RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		ret = poll_idle_status(SOCFPGA_RSTMGR(HDSKACK),
+				RSTMGR_HDSKACK_FPGAHSREQ,
+				RSTMGR_HDSKACK_FPGAHSREQ, 300);
+
+		if (ret < 0) {
+			ERROR("F2S Bridge disable: Timeout waiting for idle req\n");
+			assert(false);
+		}
+
+		/* Disable f2s bridge */
+		mmio_clrbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+				f2s_en);
+		udelay(5);
+
+		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+				f2s_force_drain);
+		udelay(5);
+
+		do {
+			/* Read response queue status to ensure it is empty */
+			uint32_t idle_status;
+
+			idle_status = mmio_read_32(SOCFPGA_F2SDRAMMGR(
+				SIDEBANDMGR_FLAGINSTATUS0));
+			if (idle_status & f2s_respempty) {
+				idle_status = mmio_read_32(SOCFPGA_F2SDRAMMGR(
+					SIDEBANDMGR_FLAGINSTATUS0));
+				if (idle_status & f2s_respempty) {
+					break;
+				}
+			}
+			udelay(1000);
+		} while (timeout-- > 0);
+
+		/* Assert reset to f2s bridge */
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+				brg_mask);
+
+		/* Clear idle request to FPGA */
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+				RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Clear idle request to MPFE */
+		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTCLR0),
+				f2s_idlereq);
+
+		/* When FPGA reconfig is complete */
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
+
+		/* Enable f2s bridge */
+		mmio_clrbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+			f2s_idlereq);
+
+		ret = poll_idle_status(SOCFPGA_F2SDRAMMGR(
+			SIDEBANDMGR_FLAGINSTATUS0), f2s_idleack, 0, 300);
+		if (ret < 0) {
+			ERROR("F2S bridge enable: Timeout waiting for idle ack");
+			assert(false);
+		}
+
+		mmio_clrbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+			f2s_force_drain);
+		udelay(5);
+
+		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTSET0),
+			f2s_en);
+		udelay(5);
+	}
+
+	return ret;
+}
+
 int socfpga_bridges_enable(uint32_t mask)
 {
 	int ret = 0;
@@ -209,9 +415,87 @@
 	uint32_t f2s_idleack = 0;
 	uint32_t f2s_respempty = 0;
 	uint32_t f2s_cmdidle = 0;
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	uint32_t delay = 0;
+#endif
 
 	/* Enable s2f bridge */
 	socfpga_s2f_bridge_mask(mask, &brg_mask, &noc_mask);
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	/* Enable SOC2FPGA bridge */
+	if (brg_mask & RSTMGR_BRGMODRSTMASK_SOC2FPGA) {
+		/* Write Reset Manager hdskreq[soc2fpga_flush_req] = 1 */
+		NOTICE("Set S2F hdskreq ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_SOC2FPGAREQ);
+
+		/* Read Reset Manager hdskack[soc2fpga] = 1 */
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_SOC2FPGAACK, RSTMGR_HDSKACK_SOC2FPGAACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("S2F bridge enable: Timeout hdskack\n");
+		}
+
+		/* Write Reset Manager hdskreq[soc2fpga_flush_req] = 0 */
+		NOTICE("Clear S2F hdskreq ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_SOC2FPGAREQ);
+
+		/* Write Reset Manager brgmodrst[soc2fpga] = 1 */
+		NOTICE("Assert S2F ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_SOC2FPGA);
+
+		/* ToDo: Shall use udelay for product release */
+		for (delay = 0; delay < 1000; delay++) {
+			/* dummy delay */
+		}
+
+		/* Write Reset Manager brgmodrst[soc2fpga] = 0 */
+		NOTICE("Deassert S2F ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_SOC2FPGA);
+	}
+
+	/* Enable LWSOC2FPGA bridge */
+	if (brg_mask & RSTMGR_BRGMODRSTMASK_LWHPS2FPGA) {
+		/* Write Reset Manager hdskreq[lwsoc2fpga_flush_req] = 1 */
+		NOTICE("Set LWS2F hdskreq ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_LWSOC2FPGAREQ);
+
+		/* Read Reset Manager hdskack[lwsoc2fpga] = 1 */
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_LWSOC2FPGAACK, RSTMGR_HDSKACK_LWSOC2FPGAACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("LWS2F bridge enable: Timeout hdskack\n");
+		}
+
+		/* Write Reset Manager hdskreq[lwsoc2fpga_flush_req] = 0 */
+		NOTICE("Clear LWS2F hdskreq ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_LWSOC2FPGAREQ);
+
+		/* Write Reset Manager brgmodrst[lwsoc2fpga] = 1 */
+		NOTICE("Assert LWS2F ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_LWHPS2FPGA);
+
+		/* ToDo: Shall use udelay for product release */
+		for (delay = 0; delay < 1000; delay++) {
+			/* dummy delay */
+		}
+
+		/* Write Reset Manager brgmodrst[lwsoc2fpga] = 0 */
+		NOTICE("Deassert LWS2F ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_LWHPS2FPGA);
+	}
+#else
 	if (brg_mask != 0U) {
 		/* Clear idle request */
 		mmio_setbits_32(SOCFPGA_SYSMGR(NOC_IDLEREQ_CLR),
@@ -224,15 +508,186 @@
 		ret = poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLEACK),
 				       noc_mask, 0, 300);
 		if (ret < 0) {
-			ERROR("S2F bridge enable: "
-					"Timeout waiting for idle ack\n");
+			ERROR("S2F bridge enable: Timeout idle ack\n");
 		}
 	}
+#endif
 
 	/* Enable f2s bridge */
 	socfpga_f2s_bridge_mask(mask, &brg_mask, &f2s_idlereq,
 				&f2s_force_drain, &f2s_en,
 				&f2s_idleack, &f2s_respempty, &f2s_cmdidle);
+#if PLATFORM_MODEL == PLAT_SOCFPGA_AGILEX5
+	/* Enable FPGA2SOC bridge */
+	if (brg_mask & RSTMGR_BRGMODRSTMASK_FPGA2SOC) {
+		/* Write Reset Manager hdsken[fpgahsen] = 1 */
+		NOTICE("Set FPGA hdsken(fpgahsen) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKEN), RSTMGR_HDSKEN_FPGAHSEN);
+
+		/* Write Reset Manager hdskreq[fpgahsreq] = 1 */
+		NOTICE("Set FPGA hdskreq(fpgahsreq) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Read Reset Manager hdskack[fpgahsack] = 1 */
+		NOTICE("Get FPGA hdskack(fpgahsack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGAHSACK, RSTMGR_HDSKACK_FPGAHSACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("FPGA bridge fpga handshake fpgahsreq: Timeout\n");
+		}
+
+		/* Write Reset Manager hdskreq[f2s_flush_req] = 1 */
+		NOTICE("Set F2S hdskreq(f2s_flush_req) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_FPGA2SOCREQ);
+
+		/* Read Reset Manager hdskack[f2s_flush_ack] = 1 */
+		NOTICE("Get F2S hdskack(f2s_flush_ack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGA2SOCACK, RSTMGR_HDSKACK_FPGA2SOCACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2S bridge fpga handshake f2sdram_flush_req: Timeout\n");
+		}
+
+		/* Write Reset Manager hdskreq[fpgahsreq] = 1 */
+		NOTICE("Clear FPGA hdskreq(fpgahsreq) ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Write Reset Manager hdskreq[f2s_flush_req] = 1 */
+		NOTICE("Clear F2S hdskreq(f2s_flush_req) ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_FPGA2SOCREQ);
+
+		/* Read Reset Manager hdskack[f2s_flush_ack] = 0 */
+		NOTICE("Get F2SDRAM hdskack(f2s_flush_ack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGA2SOCACK, RSTMGR_HDSKACK_FPGA2SOCACK_DASRT,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2S bridge fpga handshake f2s_flush_ack: Timeout\n");
+		}
+
+		/* Read Reset Manager hdskack[fpgahsack] = 0 */
+		NOTICE("Get FPGA hdskack(fpgahsack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGAHSACK, RSTMGR_HDSKACK_FPGAHSACK_DASRT,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2S bridge fpga handshake fpgahsack: Timeout\n");
+		}
+
+		/* Write Reset Manager brgmodrst[fpga2soc] = 1 */
+		NOTICE("Assert F2S ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST), RSTMGR_BRGMODRST_FPGA2SOC);
+
+		/* ToDo: Shall use udelay for product release */
+		for (delay = 0; delay < 1000; delay++) {
+			/* dummy delay */
+		}
+
+		/* Write Reset Manager brgmodrst[fpga2soc] = 0 */
+		NOTICE("Deassert F2S ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), RSTMGR_BRGMODRST_FPGA2SOC);
+
+		/* Write System Manager f2s bridge control register[f2soc_enable] = 1 */
+		NOTICE("Deassert F2S f2soc_enable ...\n");
+		mmio_setbits_32(SOCFPGA_SYSMGR(F2S_BRIDGE_CTRL),
+			SYSMGR_F2S_BRIDGE_CTRL_EN);
+	}
+
+	/* Enable FPGA2SDRAM bridge */
+	if (brg_mask & RSTMGR_BRGMODRSTMASK_F2SDRAM0) {
+		/* Write Reset Manager hdsken[fpgahsen] = 1 */
+		NOTICE("Set F2SDRAM hdsken(fpgahsen) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKEN), RSTMGR_HDSKEN_FPGAHSEN);
+
+		/* Write Reset Manager hdskreq[fpgahsreq] = 1 */
+		NOTICE("Set F2SDRAM hdskreq(fpgahsreq) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Read Reset Manager hdskack[fpgahsack] = 1 */
+		NOTICE("Get F2SDRAM hdskack(fpgahsack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGAHSACK, RSTMGR_HDSKACK_FPGAHSACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2SDRAM bridge fpga handshake fpgahsreq: Timeout\n");
+		}
+
+		/* Write Reset Manager hdskreq[f2sdram_flush_req] = 1 */
+		NOTICE("Set F2SDRAM hdskreq(f2sdram_flush_req) ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(HDSKREQ),
+			RSTMGR_HDSKREQ_F2SDRAM0REQ);
+
+		/* Read Reset Manager hdskack[f2sdram_flush_ack] = 1 */
+		NOTICE("Get F2SDRAM hdskack(f2sdram_flush_ack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_F2SDRAM0ACK, RSTMGR_HDSKACK_F2SDRAM0ACK,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2SDRAM bridge fpga handshake f2sdram_flush_req: Timeout\n");
+		}
+
+		/* Write Reset Manager hdskreq[fpgahsreq] = 1 */
+		NOTICE("Clear F2SDRAM hdskreq(fpgahsreq) ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_FPGAHSREQ);
+
+		/* Write Reset Manager hdskreq[f2sdram_flush_req] = 1 */
+		NOTICE("Clear F2SDRAM hdskreq(f2sdram_flush_req) ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(HDSKREQ), RSTMGR_HDSKREQ_F2SDRAM0REQ);
+
+		/* Read Reset Manager hdskack[f2sdram_flush_ack] = 0 */
+		NOTICE("Get F2SDRAM hdskack(f2sdram_flush_ack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_F2SDRAM0ACK, RSTMGR_HDSKACK_F2SDRAM0ACK_DASRT,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2SDRAM bridge fpga handshake f2sdram_flush_ack: Timeout\n");
+		}
+
+		/* Read Reset Manager hdskack[fpgahsack] = 0 */
+		NOTICE("Get F2SDRAM hdskack(fpgahsack) ...\n");
+		ret = poll_idle_status_by_counter(SOCFPGA_RSTMGR(HDSKACK),
+			RSTMGR_HDSKACK_FPGAHSACK, RSTMGR_HDSKACK_FPGAHSACK_DASRT,
+			300);
+
+		if (ret < 0) {
+			ERROR("F2SDRAM bridge fpga handshake fpgahsack: Timeout\n");
+		}
+
+		/* Write Reset Manager brgmodrst[fpga2sdram] = 1 */
+		NOTICE("Assert F2SDRAM ...\n");
+		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_F2SSDRAM0);
+
+		/* ToDo: Shall use udelay for product release */
+		for (delay = 0; delay < 1000; delay++) {
+			/* dummy delay */
+		}
+
+		/* Write Reset Manager brgmodrst[fpga2sdram] = 0 */
+		NOTICE("Deassert F2SDRAM ...\n");
+		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST),
+			RSTMGR_BRGMODRST_F2SSDRAM0);
+
+		/*
+		 * Clear fpga2sdram_manager_main_SidebandManager_FlagOutClr0
+		 * f2s_ready_latency_enable
+		 */
+		NOTICE("Clear F2SDRAM f2s_ready_latency_enable ...\n");
+		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTCLR0),
+			FLAGOUTCLR0_F2SDRAM0_ENABLE);
+	}
+#else
 	if (brg_mask != 0U) {
 		mmio_clrbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
 
@@ -243,8 +698,7 @@
 				       f2s_idleack, 0, 300);
 
 		if (ret < 0) {
-			ERROR("F2S bridge enable: "
-			      "Timeout waiting for idle ack");
+			ERROR("F2S bridge enable: Timeout idle ack");
 		}
 
 		/* Clear the force drain */
@@ -256,7 +710,7 @@
 				f2s_en);
 		udelay(5);
 	}
-
+#endif
 	return ret;
 }
 
@@ -329,15 +783,13 @@
 		ret = poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLEACK),
 				       noc_mask, noc_mask, 300);
 		if (ret < 0) {
-			ERROR("S2F Bridge disable: "
-			      "Timeout waiting for idle ack\n");
+			ERROR("S2F Bridge disable: Timeout idle ack\n");
 		}
 
 		ret = poll_idle_status(SOCFPGA_SYSMGR(NOC_IDLESTATUS),
 				       noc_mask, noc_mask, 300);
 		if (ret < 0) {
-			ERROR("S2F Bridge disable: "
-			      "Timeout waiting for idle status\n");
+			ERROR("S2F Bridge disable: Timeout idle status\n");
 		}
 
 		mmio_setbits_32(SOCFPGA_RSTMGR(BRGMODRST), brg_mask);
@@ -365,8 +817,8 @@
 				RSTMGR_HDSKREQ_FPGAHSREQ);
 
 		ret = poll_idle_status_by_clkcycles(SOCFPGA_RSTMGR(HDSKACK),
-						    RSTMGR_HDSKACK_FPGAHSACK_MASK,
-						    RSTMGR_HDSKACK_FPGAHSACK_MASK, 1000);
+						    RSTMGR_HDSKACK_FPGAHSREQ,
+						    RSTMGR_HDSKACK_FPGAHSREQ, 1000);
 
 		/* DISABLE F2S Bridge */
 		mmio_setbits_32(SOCFPGA_F2SDRAMMGR(SIDEBANDMGR_FLAGOUTCLR0),
@@ -394,3 +846,68 @@
 
 	return ret;
 }
+
+/* CPUxRESETBASELOW */
+int socfpga_cpu_reset_base(unsigned int cpu_id)
+{
+	int ret = 0;
+	uint32_t entrypoint = 0;
+
+	ret = socfpga_cpurstrelease(cpu_id);
+
+	if (ret < 0) {
+		return RSTMGR_RET_ERROR;
+	}
+
+	if (ret == RSTMGR_RET_OK) {
+
+		switch (cpu_id) {
+		case 0:
+			entrypoint = mmio_read_32(SOCFPGA_RSTMGR_CPUBASELOW_0);
+			entrypoint |= mmio_read_32(SOCFPGA_RSTMGR_CPUBASEHIGH_0) << 24;
+		break;
+
+		case 1:
+			entrypoint = mmio_read_32(SOCFPGA_RSTMGR_CPUBASELOW_1);
+			entrypoint |= mmio_read_32(SOCFPGA_RSTMGR_CPUBASEHIGH_1) << 24;
+		break;
+
+		case 2:
+			entrypoint = mmio_read_32(SOCFPGA_RSTMGR_CPUBASELOW_2);
+			entrypoint |= mmio_read_32(SOCFPGA_RSTMGR_CPUBASEHIGH_2) << 24;
+		break;
+
+		case 3:
+			entrypoint = mmio_read_32(SOCFPGA_RSTMGR_CPUBASELOW_3);
+			entrypoint |= mmio_read_32(SOCFPGA_RSTMGR_CPUBASEHIGH_3) << 24;
+		break;
+
+		default:
+		break;
+		}
+
+		mmio_write_64(PLAT_SEC_ENTRY, entrypoint);
+	}
+
+	return RSTMGR_RET_OK;
+}
+
+/* CPURSTRELEASE */
+int socfpga_cpurstrelease(unsigned int cpu_id)
+{
+	unsigned int timeout = 0;
+
+	do {
+		/* Read response queue status to ensure it is empty */
+		uint32_t cpurstrelease_status;
+
+		cpurstrelease_status = mmio_read_32(SOCFPGA_RSTMGR(CPURSTRELEASE));
+
+		if ((cpurstrelease_status & RSTMGR_CPUSTRELEASE_CPUx) == cpu_id) {
+			return RSTMGR_RET_OK;
+		}
+		udelay(1000);
+	} while (timeout-- > 0);
+
+	return RSTMGR_RET_ERROR;
+}