[PATCH 3/5] arm: socfpga: soc64: Move bridges reset to common function

[Jit Loon Lim <jit.loon.lim@intel.com>]

From: Ley Foon Tan <ley.foon.tan@intel.com>

HSD #1508586908-3: Move bridges reset code to common function, socfpga_s2f_bridges_reset().
This function is an inline function and can be included in normal U-boot
and psci secure section.

Signed-off-by: Ley Foon Tan <ley.foon.tan@intel.com>
Signed-off-by: Jit Loon Lim <jit.loon.lim@intel.com>
---
 arch/arm/mach-socfpga/reset_manager_s10.c | 71 +++++++++++------------
 1 file changed, 35 insertions(+), 36 deletions(-)

diff --git a/arch/arm/mach-socfpga/reset_manager_s10.c b/arch/arm/mach-socfpga/reset_manager_s10.c
index f47fec10a0..20ab374881 100644
--- a/arch/arm/mach-socfpga/reset_manager_s10.c
+++ b/arch/arm/mach-socfpga/reset_manager_s10.c
@@ -57,32 +57,20 @@ void socfpga_per_reset_all(void)
 	writel(0xffffffff, socfpga_get_rstmgr_addr() + RSTMGR_SOC64_PER1MODRST);
 }
 
-void socfpga_bridges_reset(int enable)
+static __always_inline void socfpga_s2f_bridges_reset(int enable)
 {
-#if !defined(CONFIG_SPL_BUILD) && defined(CONFIG_SPL_ATF)
-	u64 arg = enable;
-
-	int ret = invoke_smc(INTEL_SIP_SMC_HPS_SET_BRIDGES, &arg, 1, NULL, 0);
-	if (ret) {
-		printf("SMC call failed with error %d in %s.\n", ret, __func__);
-		return;
-	}
-#else
-	u32 reg;
-
 	if (enable) {
 		/* clear idle request to all bridges */
 		setbits_le32(socfpga_get_sysmgr_addr() +
 			     SYSMGR_SOC64_NOC_IDLEREQ_CLR, ~0);
 
-		/* Release all bridges from reset state */
+		/* Release SOC2FPGA bridges from reset state */
 		clrbits_le32(socfpga_get_rstmgr_addr() + RSTMGR_SOC64_BRGMODRST,
-			     ~0);
+			     BRGMODRST_SOC2FPGA_BRIDGES);
 
 		/* Poll until all idleack to 0 */
-		read_poll_timeout(readl, reg, !reg, 1000, 300000,
-				  socfpga_get_sysmgr_addr() +
-				  SYSMGR_SOC64_NOC_IDLEACK);
+		POLL_FOR_ZERO(readl(socfpga_get_sysmgr_addr() +
+			      SYSMGR_SOC64_NOC_IDLEACK), 300);
 	} else {
 		/* set idle request to all bridges */
 		writel(~0,
@@ -93,30 +81,41 @@ void socfpga_bridges_reset(int enable)
 		writel(1, socfpga_get_sysmgr_addr() + SYSMGR_SOC64_NOC_TIMEOUT);
 
 		/* Poll until all idleack to 1 */
-		read_poll_timeout(readl, reg,
-				  reg == (SYSMGR_NOC_H2F_MSK |
-					  SYSMGR_NOC_LWH2F_MSK),
-				  1000, 300000,
-				  socfpga_get_sysmgr_addr() +
-				  SYSMGR_SOC64_NOC_IDLEACK);
-
-		/* Poll until all idlestatus to 1 */
-		read_poll_timeout(readl, reg,
-				  reg == (SYSMGR_NOC_H2F_MSK |
-					  SYSMGR_NOC_LWH2F_MSK),
-				  1000, 300000,
-				  socfpga_get_sysmgr_addr() +
-				  SYSMGR_SOC64_NOC_IDLESTATUS);
-
-		/* Reset all bridges (except NOR DDR scheduler & F2S) */
+		POLL_FOR_ZERO(readl(socfpga_get_sysmgr_addr() +
+				    SYSMGR_SOC64_NOC_IDLEACK) ^
+			      (SYSMGR_NOC_H2F_MSK | SYSMGR_NOC_LWH2F_MSK),
+			      300);
+
+		POLL_FOR_ZERO(readl(socfpga_get_sysmgr_addr() +
+				    SYSMGR_SOC64_NOC_IDLESTATUS) ^
+			      (SYSMGR_NOC_H2F_MSK | SYSMGR_NOC_LWH2F_MSK),
+			      300);
+
+		/* Reset all SOC2FPGA bridges (except NOR DDR scheduler & F2S) */
 		setbits_le32(socfpga_get_rstmgr_addr() + RSTMGR_SOC64_BRGMODRST,
-			     ~(RSTMGR_BRGMODRST_DDRSCH_MASK |
-			       RSTMGR_BRGMODRST_FPGA2SOC_MASK));
+			     BRGMODRST_SOC2FPGA_BRIDGES);
 
 		/* Disable NOC timeout */
 		writel(0, socfpga_get_sysmgr_addr() + SYSMGR_SOC64_NOC_TIMEOUT);
 	}
-#endif
+}
+
+void socfpga_bridges_reset(int enable)
+{
+	if (!IS_ENABLED(CONFIG_SPL_BUILD) && IS_ENABLED(CONFIG_SPL_ATF)) {
+		u64 arg = enable;
+
+		if (invoke_smc(INTEL_SIP_SMC_HPS_SET_BRIDGES, &arg, 1, NULL, 0))
+			hang();
+	} else {
+		socfpga_s2f_bridges_reset(enable);
+
+	}
+}
+
+void __secure socfpga_bridges_reset_psci(int enable)
+{
+	socfpga_s2f_bridges_reset(enable);
 }
 
 /*
-- 
2.26.2