[PATCH 5/5] arm: socfpga: soc64: Avoid hang in bridge reset

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

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

HSD #1508586908-6: Software shouldn't hang the system if the HPS bridge reset is failed.
Change hang() to error message.

Signed-off-by: Ley Foon Tan <ley.foon.tan@intel.com>
Signed-off-by: Jit Loon Lim <jit.loon.lim@intel.com>
---
 .../include/mach/base_addr_soc64.h            |  1 +
 arch/arm/mach-socfpga/include/mach/timer.h    | 31 +++++++++++++++++++
 arch/arm/mach-socfpga/reset_manager_s10.c     | 12 +++++--
 3 files changed, 41 insertions(+), 3 deletions(-)

diff --git a/arch/arm/mach-socfpga/include/mach/base_addr_soc64.h b/arch/arm/mach-socfpga/include/mach/base_addr_soc64.h
index 3f899fcfa3..c0de59f279 100644
--- a/arch/arm/mach-socfpga/include/mach/base_addr_soc64.h
+++ b/arch/arm/mach-socfpga/include/mach/base_addr_soc64.h
@@ -16,6 +16,7 @@
 #else
 #define SOCFPGA_FW_MPU_DDR_SCR_ADDRESS		0xf8020100
 #endif
+#define SOCFPGA_F2SDRAM_MGR_ADDRESS		0xf8024000
 #define SOCFPGA_SMMU_ADDRESS			0xfa000000
 #define SOCFPGA_MAILBOX_ADDRESS			0xffa30000
 #define SOCFPGA_UART0_ADDRESS			0xffc02000
diff --git a/arch/arm/mach-socfpga/include/mach/timer.h b/arch/arm/mach-socfpga/include/mach/timer.h
index 82596e412e..be56b4968f 100644
--- a/arch/arm/mach-socfpga/include/mach/timer.h
+++ b/arch/arm/mach-socfpga/include/mach/timer.h
@@ -6,6 +6,9 @@
 #ifndef _SOCFPGA_TIMER_H_
 #define _SOCFPGA_TIMER_H_
 
+#include <asm/barriers.h>
+#include <div64.h>
+
 struct socfpga_timer {
 	u32	load_val;
 	u32	curr_val;
@@ -14,4 +17,32 @@ struct socfpga_timer {
 	u32	int_stat;
 };
 
+static __always_inline u64 __socfpga_get_time_stamp(void)
+{
+	u64 cntpct;
+
+	isb();
+	asm volatile("mrs %0, cntpct_el0" : "=r" (cntpct));
+	return cntpct;
+}
+
+static __always_inline u64 __socfpga_usec_to_tick(u64 usec)
+{
+	u64 tick = usec;
+	u64 cntfrq;
+
+	asm volatile("mrs %0, cntfrq_el0" : "=r" (cntfrq));
+	tick *= cntfrq;
+	do_div(tick, 1000000);
+	return tick;
+}
+
+static __always_inline void __socfpga_udelay(u64 usec)
+{
+	u64 tmp = __socfpga_get_time_stamp() + __socfpga_usec_to_tick(usec);
+
+	while (__socfpga_get_time_stamp() < tmp + 1)
+		;
+}
+
 #endif
diff --git a/arch/arm/mach-socfpga/reset_manager_s10.c b/arch/arm/mach-socfpga/reset_manager_s10.c
index 690cfb2b20..0274c4dbdb 100644
--- a/arch/arm/mach-socfpga/reset_manager_s10.c
+++ b/arch/arm/mach-socfpga/reset_manager_s10.c
@@ -8,13 +8,15 @@
 #include <hang.h>
 #include <asm/global_data.h>
 #include <asm/io.h>
+#include <asm/secure.h>
 #include <asm/arch/reset_manager.h>
 #include <asm/arch/smc_api.h>
 #include <asm/arch/system_manager.h>
+#include <asm/arch/timer.h>
 #include <dt-bindings/reset/altr,rst-mgr-s10.h>
+#include <exports.h>
 #include <linux/iopoll.h>
 #include <linux/intel-smc.h>
-
 DECLARE_GLOBAL_DATA_PTR;
 
 /* F2S manager registers */
@@ -223,9 +225,13 @@ void socfpga_bridges_reset(int enable)
 {
 	if (!IS_ENABLED(CONFIG_SPL_BUILD) && IS_ENABLED(CONFIG_SPL_ATF)) {
 		u64 arg = enable;
+		int ret;
 
-		if (invoke_smc(INTEL_SIP_SMC_HPS_SET_BRIDGES, &arg, 1, NULL, 0))
-			hang();
+		ret = invoke_smc(INTEL_SIP_SMC_HPS_SET_BRIDGES, &arg, 1, NULL,
+				 0);
+		if (ret)
+			printf("Failed to %s the HPS bridges, error %d\n",
+			       enable ? "enable" : "disable", ret);
 	} else {
 		socfpga_s2f_bridges_reset(enable);
 		socfpga_f2s_bridges_reset(enable);
-- 
2.26.2