From 1b56392bcb9ad470e2a2c2d122cd9eca4f7e3071 Mon Sep 17 00:00:00 2001
From: Vandhiadevan Karunamoorthy <vkarunam@codeaurora.org>
Date: Thu, 12 Dec 2019 23:06:19 +0530
Subject: [PATCH] ipq5018: Enable PSCI for reset

Change-Id: Ide99470756e91f77c267398b2b491714ced38054
Signed-off-by: Vandhiadevan Karunamoorthy <vkarunam@codeaurora.org>
---
 board/qca/arm/ipq5018/ipq5018.c | 36 +++++++++++++++++++++++++++++----
 board/qca/arm/ipq5018/ipq5018.h |  9 +++++++++
 include/configs/ipq5018.h       |  5 +++++
 3 files changed, 46 insertions(+), 4 deletions(-)

diff --git a/board/qca/arm/ipq5018/ipq5018.c b/board/qca/arm/ipq5018/ipq5018.c
index 90d4ce3981..5af37ab7e4 100644
--- a/board/qca/arm/ipq5018/ipq5018.c
+++ b/board/qca/arm/ipq5018/ipq5018.c
@@ -26,6 +26,7 @@
 #include <mmc.h>
 #include <sdhci.h>
 
+#define DLOAD_MAGIC_COOKIE	0x10
 DECLARE_GLOBAL_DATA_PTR;
 struct sdhci_host mmc_host;
 extern int ipq_spi_init(u16);
@@ -287,9 +288,40 @@ int board_mmc_init(bd_t *bis)
 
 void reset_crashdump(void)
 {
+	unsigned int ret = 0;
+	qca_scm_sdi();
+	ret = qca_scm_dload(CLEAR_MAGIC);
+	if (ret)
+		printf ("Error in reseting the Magic cookie\n");
 	return;
 }
 
+void psci_sys_reset(void)
+{
+	__invoke_psci_fn_smc(0x84000009, 0, 0, 0);
+}
+
+void qti_scm_pshold(void)
+{
+	int ret;
+
+	ret = scm_call(SCM_SVC_BOOT, SCM_CMD_TZ_PSHOLD, NULL, 0, NULL, 0);
+
+	if (ret != 0)
+		writel(0, GCNT_PSHOLD);
+}
+
+void reset_cpu(unsigned long a)
+{
+	reset_crashdump();
+	if (is_scm_armv8()) {
+		psci_sys_reset();
+	} else {
+		qti_scm_pshold();
+	}
+	while(1);
+}
+
 void board_nand_init(void)
 {
 #ifdef CONFIG_QCA_SPI
@@ -327,7 +359,3 @@ unsigned long timer_read_counter(void)
 	return 0;
 }
 
-void reset_cpu(unsigned long a)
-{
-	while(1);
-}
diff --git a/board/qca/arm/ipq5018/ipq5018.h b/board/qca/arm/ipq5018/ipq5018.h
index a220312d94..f5ccb5ec95 100644
--- a/board/qca/arm/ipq5018/ipq5018.h
+++ b/board/qca/arm/ipq5018/ipq5018.h
@@ -77,6 +77,15 @@
 #define KERNEL_AUTH_CMD                   0x13
 #define SCM_CMD_SEC_AUTH		       0x1F
 
+#define ARM_PSCI_TZ_FN_BASE		0x84000000
+#define ARM_PSCI_TZ_FN(n)		(ARM_PSCI_TZ_FN_BASE + (n))
+
+#define ARM_PSCI_TZ_FN_CPU_OFF		ARM_PSCI_TZ_FN(2)
+#define ARM_PSCI_TZ_FN_CPU_ON		ARM_PSCI_TZ_FN(3)
+#define ARM_PSCI_TZ_FN_AFFINITY_INFO	ARM_PSCI_TZ_FN(4)
+
+unsigned int __invoke_psci_fn_smc(unsigned int, unsigned int,
+					 unsigned int, unsigned int);
 struct smem_ram_ptn {
 	char name[16];
 	unsigned long long start;
diff --git a/include/configs/ipq5018.h b/include/configs/ipq5018.h
index e9a893ebe4..10b40dd755 100644
--- a/include/configs/ipq5018.h
+++ b/include/configs/ipq5018.h
@@ -33,6 +33,11 @@
 
 #define CONFIG_ENV_SIZE_MAX			(256 << 10) /* 256 KB */
 
+/*
+* PSCI Calls enable
+*/
+#define CONFIG_ARMV7_PSCI
+
 /*
  *Size of malloc() pool
  */