fix(cpus): check for SME presence in Gelas
The original powerdown function for Gelas included SME disabling
instructions but did not check for the presence of SME before disabling.
This could lead to unexpected beaviors. This patch adds that check so
the feature is disabled only if it is present.
Change-Id: I582db53a6669317620e4f72a3eac87525897d3d0
Signed-off-by: Juan Pablo Conde <juanpablo.conde@arm.com>
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index bc37c94..951f023 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -437,6 +437,7 @@
#define ID_AA64PFR1_EL1_SME_SHIFT U(24)
#define ID_AA64PFR1_EL1_SME_MASK ULL(0xf)
+#define ID_AA64PFR1_EL1_SME_WIDTH U(4)
#define ID_AA64PFR1_EL1_SME_NOT_SUPPORTED ULL(0x0)
#define ID_AA64PFR1_EL1_SME_SUPPORTED ULL(0x1)
#define ID_AA64PFR1_EL1_SME2_SUPPORTED ULL(0x2)
diff --git a/lib/cpus/aarch64/cortex_gelas.S b/lib/cpus/aarch64/cortex_gelas.S
index e0d20a9..dc704f2 100644
--- a/lib/cpus/aarch64/cortex_gelas.S
+++ b/lib/cpus/aarch64/cortex_gelas.S
@@ -34,13 +34,20 @@
* ----------------------------------------------------
*/
func cortex_gelas_core_pwr_dwn
+#if ENABLE_SME_FOR_NS
/* ---------------------------------------------------
- * Disable SME
+ * Disable SME if enabled and supported
* ---------------------------------------------------
*/
+ mrs x0, ID_AA64PFR1_EL1
+ ubfx x0, x0, #ID_AA64PFR1_EL1_SME_SHIFT, \
+ #ID_AA64PFR1_EL1_SME_WIDTH
+ cmp x0, #ID_AA64PFR1_EL1_SME_NOT_SUPPORTED
+ b.eq 1f
msr CORTEX_GELAS_SVCRSM, xzr
msr CORTEX_GELAS_SVCRZA, xzr
-
+1:
+#endif
/* ---------------------------------------------------
* Enable CPU power down bit in power control register
* ---------------------------------------------------