Tegra194: Enable fake system suspend

Fake system suspend for Tegra194, calls the routine
tegra_secure_entrypoint() instead of calling WFI.
In essence, this is a debug mode that ensures
that the code path of kernel->ATF and back to kernel
is executed without depending on other components
involved in the system suspend path.

This is for ensuring that verification of system suspend
can be done on pre-silicon platforms without depending on
the rest of the layers being enabled.

Change-Id: I18572b169b7ef786f9029600dad9ef5728634f2b
Signed-off-by: Vignesh Radhakrishnan <vigneshr@nvidia.com>

2 files changed, 68 insertions, 10 deletions

diff --git a/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c b/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c
index 2b45ee84f..8208df01e 100644
--- a/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c
+++ b/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c
@@ -19,10 +19,11 @@
 #include <string.h>
 #include <tegra_private.h>
 #include <t194_nvg.h>
+#include <stdbool.h>
 
 extern void prepare_core_pwr_dwn(void);
 
-extern uint8_t tegra_fake_system_suspend;
+extern void tegra_secure_entrypoint(void);
 
 #if ENABLE_SYSTEM_SUSPEND_CTX_SAVE_TZDRAM
 extern void tegra186_cpu_reset_handler(void);
@@ -48,6 +49,14 @@ static struct t18x_psci_percpu_data {
 	unsigned int wake_time;
 } __aligned(CACHE_WRITEBACK_GRANULE) percpu_data[PLATFORM_CORE_COUNT];
 
+/*
+ * tegra_fake_system_suspend acts as a boolean var controlling whether
+ * we are going to take fake system suspend code or normal system suspend code
+ * path. This variable is set inside the sip call handlers, when the kernel
+ * requests an SIP call to set the suspend debug flags.
+ */
+bool tegra_fake_system_suspend;
+
 int32_t tegra_soc_validate_power_state(unsigned int power_state,
 					psci_power_state_t *req_state)
 {
@@ -96,8 +105,14 @@ int tegra_soc_pwr_domain_suspend(const psci_power_state_t *target_state)
 	uint64_t smmu_ctx_base;
 #endif
 	uint32_t val;
-	mce_cstate_info_t cstate_info = { 0 };
+	mce_cstate_info_t sc7_cstate_info = {
+		.cluster = TEGRA_NVG_CLUSTER_CC6,
+		.system = TEGRA_NVG_SYSTEM_SC7,
+		.system_state_force = 1,
+		.update_wake_mask = 1,
+	};
 	int cpu = plat_my_core_pos();
+	int32_t ret = 0;
 
 	/* get the state ID */
 	pwr_domain_state = target_state->pwr_domain_state;
@@ -112,8 +127,9 @@ int tegra_soc_pwr_domain_suspend(const psci_power_state_t *target_state)
 		/* Enter CPU idle/powerdown */
 		val = (stateid_afflvl0 == PSTATE_ID_CORE_IDLE) ?
 			TEGRA_NVG_CORE_C6 : TEGRA_NVG_CORE_C7;
-		(void)mce_command_handler(MCE_CMD_ENTER_CSTATE, val,
+		ret = mce_command_handler(MCE_CMD_ENTER_CSTATE, val,
 				percpu_data[cpu].wake_time, 0);
+		assert(ret == 0);
 
 	} else if (stateid_afflvl2 == PSTATE_ID_SOC_POWERDN) {
 
@@ -140,15 +156,10 @@ int tegra_soc_pwr_domain_suspend(const psci_power_state_t *target_state)
 		tegra_smmu_save_context(0);
 #endif
 
-		if (tegra_fake_system_suspend == 0U) {
+		if (!tegra_fake_system_suspend) {
 
 			/* Prepare for system suspend */
-			cstate_info.cluster = TEGRA_NVG_CLUSTER_CC6;
-			cstate_info.system = TEGRA_NVG_SYSTEM_SC7;
-			cstate_info.system_state_force = 1;
-			cstate_info.update_wake_mask = 1;
-
-			mce_update_cstate_info(&cstate_info);
+			mce_update_cstate_info(&sc7_cstate_info);
 
 			do {
 				val = mce_command_handler(
@@ -230,6 +241,7 @@ int tegra_soc_pwr_domain_power_down_wfi(const psci_power_state_t *target_state)
 	unsigned int stateid_afflvl2 = pwr_domain_state[PLAT_MAX_PWR_LVL] &
 		TEGRA186_STATE_ID_MASK;
 	uint64_t val;
+	u_register_t ns_sctlr_el1;
 
 	if (stateid_afflvl2 == PSTATE_ID_SOC_POWERDN) {
 		/*
@@ -242,6 +254,31 @@ int tegra_soc_pwr_domain_power_down_wfi(const psci_power_state_t *target_state)
 			 (uintptr_t)tegra186_cpu_reset_handler);
 		memcpy((void *)(uintptr_t)val, (void *)(uintptr_t)BL31_BASE,
 		       (uintptr_t)&__BL31_END__ - (uintptr_t)BL31_BASE);
+
+
+		/*
+		 * In fake suspend mode, ensure that the loopback procedure
+		 * towards system suspend exit is started, instead of calling
+		 * WFI. This is done by disabling both MMU's of EL1 & El3
+		 * and calling tegra_secure_entrypoint().
+		 */
+		if (tegra_fake_system_suspend) {
+
+			/*
+			 * Disable EL1's MMU.
+			 */
+			ns_sctlr_el1 = read_sctlr_el1();
+			ns_sctlr_el1 &= (~((u_register_t)SCTLR_M_BIT));
+			write_sctlr_el1(ns_sctlr_el1);
+
+			/*
+			 * Disable MMU to power up the CPU in a "clean"
+			 * state
+			 */
+			disable_mmu_el3();
+			tegra_secure_entrypoint();
+			panic();
+		}
 	}
 
 	return PSCI_E_SUCCESS;
diff --git a/plat/nvidia/tegra/soc/t194/plat_sip_calls.c b/plat/nvidia/tegra/soc/t194/plat_sip_calls.c
index eaad73a40..4b6a901db 100644
--- a/plat/nvidia/tegra/soc/t194/plat_sip_calls.c
+++ b/plat/nvidia/tegra/soc/t194/plat_sip_calls.c
@@ -15,14 +15,19 @@
 #include <memctrl.h>
 #include <common/runtime_svc.h>
 #include <tegra_private.h>
+#include <tegra_platform.h>
+#include <stdbool.h>
 
 extern uint32_t tegra186_system_powerdn_state;
 
+extern bool tegra_fake_system_suspend;
+
 /*******************************************************************************
  * Tegra186 SiP SMCs
  ******************************************************************************/
 #define TEGRA_SIP_SYSTEM_SHUTDOWN_STATE			0xC2FFFE01
 #define TEGRA_SIP_GET_ACTMON_CLK_COUNTERS		0xC2FFFE02
+#define TEGRA_SIP_ENABLE_FAKE_SYSTEM_SUSPEND		0xC2FFFE03
 #define TEGRA_SIP_MCE_CMD_ENTER_CSTATE			0xC2FFFF00
 #define TEGRA_SIP_MCE_CMD_UPDATE_CSTATE_INFO		0xC2FFFF01
 #define TEGRA_SIP_MCE_CMD_UPDATE_CROSSOVER_TIME		0xC2FFFF02
@@ -110,6 +115,22 @@ int plat_sip_handler(uint32_t smc_fid,
 
 		return 0;
 
+	case TEGRA_SIP_ENABLE_FAKE_SYSTEM_SUSPEND:
+		/*
+		 * System suspend mode is set if the platform ATF is running is
+		 * VDK and there is a debug SIP call. This mode ensures that the
+		 * debug path is excercied, instead of regular code path to suit
+		 * the pre-silicon platform needs. These include replacing the
+		 * the call to WFI with calls to system suspend exit procedures.
+		 */
+		if (tegra_platform_is_virt_dev_kit()) {
+
+			tegra_fake_system_suspend = true;
+			return 0;
+		}
+
+		break;
+
 	default:
 		break;
 	}