Factor out extension enabling to a separate function

Factor out extension enabling to a separate function that is called
before exiting from EL3 for first entry into Non-secure world.

Change-Id: Ic21401ebba531134d08643c0a1ca9de0fc590a1b
Signed-off-by: Dimitris Papastamos <dimitris.papastamos@arm.com>

lib/el3_runtime/aarch32/context_mgmt.c

@@ -124,6 +124,17 @@ static void cm_init_context_common(cpu_context_t *ctx, const entry_point_info_t
 	memcpy((void *)reg_ctx, (void *)&ep->args, sizeof(aapcs32_params_t));
 }
 
+/*******************************************************************************
+ * Enable architecture extensions on first entry to Non-secure world.
+ * When EL2 is implemented but unused `el2_unused` is non-zero, otherwise
+ * it is zero.
+ ******************************************************************************/
+static void enable_extensions_nonsecure(int el2_unused)
+{
+#if IMAGE_BL32
+#endif
+}
+
 /*******************************************************************************
  * The following function initializes the cpu_context for a CPU specified by
  * its `cpu_idx` for first use, and sets the initial entrypoint state as
@@ -161,6 +172,7 @@ void cm_prepare_el3_exit(uint32_t security_state)
 {
 	uint32_t hsctlr, scr;
 	cpu_context_t *ctx = cm_get_context(security_state);
+	int el2_unused = 0;
 
 	assert(ctx);
 
@@ -185,6 +197,8 @@ void cm_prepare_el3_exit(uint32_t security_state)
 			isb();
 		} else if (read_id_pfr1() &
 			(ID_PFR1_VIRTEXT_MASK << ID_PFR1_VIRTEXT_SHIFT)) {
+			el2_unused = 1;
+
 			/*
 			 * Set the NS bit to access NS copies of certain banked
 			 * registers
@@ -283,5 +297,6 @@ void cm_prepare_el3_exit(uint32_t security_state)
 			write_scr(read_scr() & ~SCR_NS_BIT);
 			isb();
 		}
+		enable_extensions_nonsecure(el2_unused);
 	}
 }

lib/el3_runtime/aarch64/context_mgmt.c

@@ -208,6 +208,17 @@ static void cm_init_context_common(cpu_context_t *ctx, const entry_point_info_t
 	memcpy(gp_regs, (void *)&ep->args, sizeof(aapcs64_params_t));
 }
 
+/*******************************************************************************
+ * Enable architecture extensions on first entry to Non-secure world.
+ * When EL2 is implemented but unused `el2_unused` is non-zero, otherwise
+ * it is zero.
+ ******************************************************************************/
+static void enable_extensions_nonsecure(int el2_unused)
+{
+#if IMAGE_BL31
+#endif
+}
+
 /*******************************************************************************
  * The following function initializes the cpu_context for a CPU specified by
  * its `cpu_idx` for first use, and sets the initial entrypoint state as
@@ -245,6 +256,7 @@ void cm_prepare_el3_exit(uint32_t security_state)
 {
 	uint32_t sctlr_elx, scr_el3, mdcr_el2;
 	cpu_context_t *ctx = cm_get_context(security_state);
+	int el2_unused = 0;
 
 	assert(ctx);
 
@@ -258,6 +270,8 @@ void cm_prepare_el3_exit(uint32_t security_state)
 			sctlr_elx |= SCTLR_EL2_RES1;
 			write_sctlr_el2(sctlr_elx);
 		} else if (EL_IMPLEMENTED(2)) {
+			el2_unused = 1;
+
 			/*
 			 * EL2 present but unused, need to disable safely.
 			 * SCTLR_EL2 can be ignored in this case.
@@ -420,6 +434,7 @@ void cm_prepare_el3_exit(uint32_t security_state)
 			write_cnthp_ctl_el2(CNTHP_CTL_RESET_VAL &
 						~(CNTHP_CTL_ENABLE_BIT));
 		}
+		enable_extensions_nonsecure(el2_unused);
 	}
 
 	cm_el1_sysregs_context_restore(security_state);