Merge "fix(cm): update gic el2 sysregs save/restore mechanism" into integration
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 52e917e..922b2cf 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -1134,13 +1134,58 @@
}
#endif /* CTX_INCLUDE_MPAM_REGS */
-/* -----------------------------------------------------
+/* ---------------------------------------------------------------------------
* The following registers are not added:
- * AMEVCNTVOFF0<n>_EL2
- * AMEVCNTVOFF1<n>_EL2
* ICH_AP0R<n>_EL2
* ICH_AP1R<n>_EL2
* ICH_LR<n>_EL2
+ *
+ * NOTE: For a system with S-EL2 present but not enabled, accessing
+ * ICC_SRE_EL2 is undefined from EL3. To workaround this change the
+ * SCR_EL3.NS = 1 before accessing this register.
+ * ---------------------------------------------------------------------------
+ */
+static void el2_sysregs_context_save_gic(el2_sysregs_t *ctx)
+{
+#if defined(SPD_spmd) && SPMD_SPM_AT_SEL2
+ write_ctx_reg(ctx, CTX_ICC_SRE_EL2, read_icc_sre_el2());
+#else
+ u_register_t scr_el3 = read_scr_el3();
+ write_scr_el3(scr_el3 | SCR_NS_BIT);
+ isb();
+
+ write_ctx_reg(ctx, CTX_ICC_SRE_EL2, read_icc_sre_el2());
+
+ write_scr_el3(scr_el3);
+ isb();
+
+#endif
+ write_ctx_reg(ctx, CTX_ICH_HCR_EL2, read_ich_hcr_el2());
+ write_ctx_reg(ctx, CTX_ICH_VMCR_EL2, read_ich_vmcr_el2());
+}
+
+static void el2_sysregs_context_restore_gic(el2_sysregs_t *ctx)
+{
+#if defined(SPD_spmd) && SPMD_SPM_AT_SEL2
+ write_icc_sre_el2(read_ctx_reg(ctx, CTX_ICC_SRE_EL2));
+#else
+ u_register_t scr_el3 = read_scr_el3();
+ write_scr_el3(scr_el3 | SCR_NS_BIT);
+ isb();
+
+ write_icc_sre_el2(read_ctx_reg(ctx, CTX_ICC_SRE_EL2));
+
+ write_scr_el3(scr_el3);
+ isb();
+#endif
+ write_ich_hcr_el2(read_ctx_reg(ctx, CTX_ICH_HCR_EL2));
+ write_ich_vmcr_el2(read_ctx_reg(ctx, CTX_ICH_VMCR_EL2));
+}
+
+/* -----------------------------------------------------
+ * The following registers are not added:
+ * AMEVCNTVOFF0<n>_EL2
+ * AMEVCNTVOFF1<n>_EL2
* -----------------------------------------------------
*/
static void el2_sysregs_context_save_common(el2_sysregs_t *ctx)
@@ -1162,22 +1207,6 @@
write_ctx_reg(ctx, CTX_HCR_EL2, read_hcr_el2());
write_ctx_reg(ctx, CTX_HPFAR_EL2, read_hpfar_el2());
write_ctx_reg(ctx, CTX_HSTR_EL2, read_hstr_el2());
-
- /*
- * Set the NS bit to be able to access the ICC_SRE_EL2 register
- * TODO: remove with root context
- */
- u_register_t scr_el3 = read_scr_el3();
-
- write_scr_el3(scr_el3 | SCR_NS_BIT);
- isb();
- write_ctx_reg(ctx, CTX_ICC_SRE_EL2, read_icc_sre_el2());
-
- write_scr_el3(scr_el3);
- isb();
-
- write_ctx_reg(ctx, CTX_ICH_HCR_EL2, read_ich_hcr_el2());
- write_ctx_reg(ctx, CTX_ICH_VMCR_EL2, read_ich_vmcr_el2());
write_ctx_reg(ctx, CTX_MAIR_EL2, read_mair_el2());
write_ctx_reg(ctx, CTX_MDCR_EL2, read_mdcr_el2());
write_ctx_reg(ctx, CTX_SCTLR_EL2, read_sctlr_el2());
@@ -1212,22 +1241,6 @@
write_hcr_el2(read_ctx_reg(ctx, CTX_HCR_EL2));
write_hpfar_el2(read_ctx_reg(ctx, CTX_HPFAR_EL2));
write_hstr_el2(read_ctx_reg(ctx, CTX_HSTR_EL2));
-
- /*
- * Set the NS bit to be able to access the ICC_SRE_EL2 register
- * TODO: remove with root context
- */
- u_register_t scr_el3 = read_scr_el3();
-
- write_scr_el3(scr_el3 | SCR_NS_BIT);
- isb();
- write_icc_sre_el2(read_ctx_reg(ctx, CTX_ICC_SRE_EL2));
-
- write_scr_el3(scr_el3);
- isb();
-
- write_ich_hcr_el2(read_ctx_reg(ctx, CTX_ICH_HCR_EL2));
- write_ich_vmcr_el2(read_ctx_reg(ctx, CTX_ICH_VMCR_EL2));
write_mair_el2(read_ctx_reg(ctx, CTX_MAIR_EL2));
write_mdcr_el2(read_ctx_reg(ctx, CTX_MDCR_EL2));
write_sctlr_el2(read_ctx_reg(ctx, CTX_SCTLR_EL2));
@@ -1257,6 +1270,7 @@
el2_sysregs_ctx = get_el2_sysregs_ctx(ctx);
el2_sysregs_context_save_common(el2_sysregs_ctx);
+ el2_sysregs_context_save_gic(el2_sysregs_ctx);
if (is_feat_mte2_supported()) {
write_ctx_reg(el2_sysregs_ctx, CTX_TFSR_EL2, read_tfsr_el2());
@@ -1336,6 +1350,7 @@
el2_sysregs_ctx = get_el2_sysregs_ctx(ctx);
el2_sysregs_context_restore_common(el2_sysregs_ctx);
+ el2_sysregs_context_restore_gic(el2_sysregs_ctx);
if (is_feat_mte2_supported()) {
write_tfsr_el2(read_ctx_reg(el2_sysregs_ctx, CTX_TFSR_EL2));