Tegra194: enable dual execution for EL2 and EL3

This patch enables dual execution optimized translations for EL2 and EL3
CPU exception levels.

Change-Id: I28fe98bb05687400f247e94adf44a1f3a85c38b1
Signed-off-by: Varun Wadekar <vwadekar@nvidia.com>

4 files changed, 48 insertions, 6 deletions

diff --git a/include/lib/cpus/aarch64/denver.h b/include/lib/cpus/aarch64/denver.h
index 02657a0fb..b98abdf4d 100644
--- a/include/lib/cpus/aarch64/denver.h
+++ b/include/lib/cpus/aarch64/denver.h
@@ -34,6 +34,11 @@
 #define DENVER_CPU_PMSTATE_C7		U(0x7)
 #define DENVER_CPU_PMSTATE_MASK		U(0xF)
 
+/* ACTRL_ELx bits to enable dual execution*/
+#define DENVER_CPU_ENABLE_DUAL_EXEC_EL2 (ULL(1) << 9)
+#define DENVER_CPU_ENABLE_DUAL_EXEC_EL3 (ULL(1) << 9)
+#define DENVER_CPU_ENABLE_DUAL_EXEC_EL1 (U(1) << 4)
+
 #ifndef __ASSEMBLER__
 
 /* Disable Dynamic Code Optimisation */
diff --git a/plat/nvidia/tegra/include/tegra_private.h b/plat/nvidia/tegra/include/tegra_private.h
index ad3cee4b4..f72c9cf3c 100644
--- a/plat/nvidia/tegra/include/tegra_private.h
+++ b/plat/nvidia/tegra/include/tegra_private.h
@@ -47,6 +47,8 @@ typedef struct plat_params_from_bl2 {
 	uint64_t sc7entry_fw_size;
 	/* System Suspend Entry Firmware base address */
 	uint64_t sc7entry_fw_base;
+	/* Enable dual execution */
+	uint8_t enable_ccplex_lock_step;
 } plat_params_from_bl2_t;
 
 /*******************************************************************************
diff --git a/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c b/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c
index d92025b8b..bd8200456 100644
--- a/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c
+++ b/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c
@@ -368,7 +368,11 @@ int32_t tegra_soc_pwr_domain_on(u_register_t mpidr)
 
 int32_t tegra_soc_pwr_domain_on_finish(const psci_power_state_t *target_state)
 {
+	const plat_params_from_bl2_t *params_from_bl2 = bl31_get_plat_params();
+	uint8_t enable_ccplex_lock_step = params_from_bl2->enable_ccplex_lock_step;
 	uint8_t stateid_afflvl2 = target_state->pwr_domain_state[PLAT_MAX_PWR_LVL];
+	cpu_context_t *ctx = cm_get_context(NON_SECURE);
+	uint64_t actlr_elx;
 
 	/*
 	 * Reset power state info for CPUs when onlining, we set
@@ -446,13 +450,23 @@ int32_t tegra_soc_pwr_domain_on_finish(const psci_power_state_t *target_state)
 			mmio_write_32(TEGRA_XUSB_PADCTL_BASE +
 				XUSB_PADCTL_DEV_AXI_STREAMID_PF_0, TEGRA_SID_XUSB_DEV);
 		}
+	}
 
-		/*
-		 * Reset power state info for the last core doing SC7
-		 * entry and exit, we set deepest power state as CC7
-		 * and SC7 for SC7 entry which may not be requested by
-		 * non-secure SW which controls idle states.
-		 */
+	/*
+	 * Enable dual execution optimized translations for all ELx.
+	 */
+	if (enable_ccplex_lock_step != 0U) {
+		actlr_elx = read_actlr_el3();
+		actlr_elx |= DENVER_CPU_ENABLE_DUAL_EXEC_EL3;
+		write_actlr_el3(actlr_elx);
+
+		actlr_elx = read_actlr_el2();
+		actlr_elx |= DENVER_CPU_ENABLE_DUAL_EXEC_EL2;
+		write_actlr_el2(actlr_elx);
+
+		actlr_elx = read_actlr_el1();
+		actlr_elx |= DENVER_CPU_ENABLE_DUAL_EXEC_EL1;
+		write_actlr_el1(actlr_elx);
 	}
 
 	return PSCI_E_SUCCESS;
diff --git a/plat/nvidia/tegra/soc/t194/plat_setup.c b/plat/nvidia/tegra/soc/t194/plat_setup.c
index 235fba43c..82555403e 100644
--- a/plat/nvidia/tegra/soc/t194/plat_setup.c
+++ b/plat/nvidia/tegra/soc/t194/plat_setup.c
@@ -201,6 +201,10 @@ void plat_enable_console(int32_t id)
  ******************************************************************************/
 void plat_early_platform_setup(void)
 {
+	const plat_params_from_bl2_t *params_from_bl2 = bl31_get_plat_params();
+	uint8_t enable_ccplex_lock_step = params_from_bl2->enable_ccplex_lock_step;
+	uint64_t actlr_elx;
+
 	/* sanity check MCE firmware compatibility */
 	mce_verify_firmware_version();
 
@@ -250,6 +254,23 @@ void plat_early_platform_setup(void)
 		mmio_write_32(TEGRA_XUSB_PADCTL_BASE +
 			XUSB_PADCTL_DEV_AXI_STREAMID_PF_0, TEGRA_SID_XUSB_DEV);
 	}
+
+	/*
+	 * Enable dual execution optimized translations for all ELx.
+	 */
+	if (enable_ccplex_lock_step != 0U) {
+		actlr_elx = read_actlr_el3();
+		actlr_elx |= DENVER_CPU_ENABLE_DUAL_EXEC_EL3;
+		write_actlr_el3(actlr_elx);
+
+		actlr_elx = read_actlr_el2();
+		actlr_elx |= DENVER_CPU_ENABLE_DUAL_EXEC_EL2;
+		write_actlr_el2(actlr_elx);
+
+		actlr_elx = read_actlr_el1();
+		actlr_elx |= DENVER_CPU_ENABLE_DUAL_EXEC_EL1;
+		write_actlr_el1(actlr_elx);
+	}
 }
 
 /* Secure IRQs for Tegra194 */