2 files changed, 131 insertions, 0 deletions
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index b892729e..c6c2249a 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -18,6 +18,7 @@
 #include <smcc_helpers.h>
 #include <spe.h>
 #include <string.h>
+#include <sve.h>
 #include <utils.h>
 
 
@@ -225,6 +226,10 @@ static void enable_extensions_nonsecure(int el2_unused)
 #if ENABLE_AMU
 	amu_enable(el2_unused);
 #endif
+
+#if ENABLE_SVE_FOR_NS
+	sve_enable(el2_unused);
+#endif
 #endif
 }
 
diff --git a/lib/extensions/sve/sve.c b/lib/extensions/sve/sve.c
new file mode 100644
index 00000000..14e51bd8
--- /dev/null
+++ b/lib/extensions/sve/sve.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <pubsub.h>
+#include <sve.h>
+
+static void *disable_sve_hook(const void *arg)
+{
+	uint64_t features;
+
+	features = read_id_aa64pfr0_el1() >> ID_AA64PFR0_SVE_SHIFT;
+	if ((features & ID_AA64PFR0_SVE_MASK) == 1) {
+		uint64_t cptr;
+
+		/*
+		 * Disable SVE, SIMD and FP access for the Secure world.
+		 * As the SIMD/FP registers are part of the SVE Z-registers, any
+		 * use of SIMD/FP functionality will corrupt the SVE registers.
+		 * Therefore it is necessary to prevent use of SIMD/FP support
+		 * in the Secure world as well as SVE functionality.
+		 */
+		cptr = read_cptr_el3();
+		cptr = (cptr | TFP_BIT) & ~(CPTR_EZ_BIT);
+		write_cptr_el3(cptr);
+
+		/*
+		 * No explicit ISB required here as ERET to switch to Secure
+		 * world covers it
+		 */
+	}
+	return 0;
+}
+
+static void *enable_sve_hook(const void *arg)
+{
+	uint64_t features;
+
+	features = read_id_aa64pfr0_el1() >> ID_AA64PFR0_SVE_SHIFT;
+	if ((features & ID_AA64PFR0_SVE_MASK) == 1) {
+		uint64_t cptr;
+
+		/*
+		 * Enable SVE, SIMD and FP access for the Non-secure world.
+		 */
+		cptr = read_cptr_el3();
+		cptr = (cptr | CPTR_EZ_BIT) & ~(TFP_BIT);
+		write_cptr_el3(cptr);
+
+		/*
+		 * No explicit ISB required here as ERET to switch to Non-secure
+		 * world covers it
+		 */
+	}
+	return 0;
+}
+
+void sve_enable(int el2_unused)
+{
+	uint64_t features;
+
+	features = read_id_aa64pfr0_el1() >> ID_AA64PFR0_SVE_SHIFT;
+	if ((features & ID_AA64PFR0_SVE_MASK) == 1) {
+		uint64_t cptr;
+#if CTX_INCLUDE_FPREGS
+		/*
+		 * CTX_INCLUDE_FPREGS is not supported on SVE enabled systems.
+		 */
+		assert(0);
+#endif
+		/*
+		 * Update CPTR_EL3 to enable access to SVE functionality for the
+		 * Non-secure world.
+		 * NOTE - assumed that CPTR_EL3.TFP is set to allow access to
+		 * the SIMD, floating-point and SVE support.
+		 *
+		 * CPTR_EL3.EZ: Set to 1 to enable access to SVE  functionality
+		 *  in the Non-secure world.
+		 */
+		cptr = read_cptr_el3();
+		cptr |= CPTR_EZ_BIT;
+		write_cptr_el3(cptr);
+
+		/*
+		 * Need explicit ISB here to guarantee that update to ZCR_ELx
+		 * and CPTR_EL2.TZ do not result in trap to EL3.
+		 */
+		isb();
+
+		/*
+		 * Ensure lower ELs have access to full vector length.
+		 */
+		write_zcr_el3(ZCR_EL3_LEN_MASK);
+
+		if (el2_unused) {
+			/*
+			 * Update CPTR_EL2 to enable access to SVE functionality
+			 * for Non-secure world, EL2 and Non-secure EL1 and EL0.
+			 * NOTE - assumed that CPTR_EL2.TFP is set to allow
+			 * access to the SIMD, floating-point and SVE support.
+			 *
+			 * CPTR_EL2.TZ: Set to 0 to enable access to SVE support
+			 *  for EL2 and Non-secure EL1 and EL0.
+			 */
+			cptr = read_cptr_el2();
+			cptr &= ~(CPTR_EL2_TZ_BIT);
+			write_cptr_el2(cptr);
+
+			/*
+			 * Ensure lower ELs have access to full vector length.
+			 */
+			write_zcr_el2(ZCR_EL2_LEN_MASK);
+		}
+		/*
+		 * No explicit ISB required here as ERET to switch to
+		 * Non-secure world covers it.
+		 */
+	}
+}
+
+SUBSCRIBE_TO_EVENT(cm_exited_normal_world, disable_sve_hook);
+SUBSCRIBE_TO_EVENT(cm_entering_normal_world, enable_sve_hook);