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>
diff --git a/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c b/plat/nvidia/tegra/soc/t194/plat_psci_handlers.c
index d92025b..bd82004 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_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 @@
 			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 235fba4..8255540 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_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 @@
 		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 */