Implement {spe,sve}_supported() helpers and refactor code

Implement helpers to test if the core supports SPE/SVE.  We have a
similar helper for AMU and this patch makes all extensions consistent
in their implementation.

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

include/lib/extensions/spe.h

 /*
- * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,7 @@
 #ifndef __SPE_H__
 #define __SPE_H__
 
+int spe_supported(void);
 void spe_enable(int el2_unused);
 void spe_disable(void);
 

include/lib/extensions/sve.h

 /*
- * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,7 @@
 #ifndef __SVE_H__
 #define __SVE_H__
 
+int sve_supported(void);
 void sve_enable(int el2_unused);
 
 #endif /* __SVE_H__ */

lib/extensions/spe/spe.c

 /*
- * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,71 +14,72 @@
  */
 #define psb_csync()	asm volatile("hint #17")
 
-void spe_enable(int el2_unused)
+int spe_supported(void)
 {
 	uint64_t features;
 
 	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_PMS_SHIFT;
-	if ((features & ID_AA64DFR0_PMS_MASK) == 1) {
-		uint64_t v;
+	return (features & ID_AA64DFR0_PMS_MASK) == 1;
+}
 
-		if (el2_unused) {
-			/*
-			 * MDCR_EL2.TPMS (ARM v8.2): Do not trap statistical
-			 * profiling controls to EL2.
-			 *
-			 * MDCR_EL2.E2PB (ARM v8.2): SPE enabled in Non-secure
-			 * state. Accesses to profiling buffer controls at
-			 * Non-secure EL1 are not trapped to EL2.
-			 */
-			v = read_mdcr_el2();
-			v &= ~MDCR_EL2_TPMS;
-			v |= MDCR_EL2_E2PB(MDCR_EL2_E2PB_EL1);
-			write_mdcr_el2(v);
-		}
+void spe_enable(int el2_unused)
+{
+	uint64_t v;
 
+	if (!spe_supported())
+		return;
+
+	if (el2_unused) {
 		/*
-		 * MDCR_EL2.NSPB (ARM v8.2): SPE enabled in Non-secure state
-		 * and disabled in secure state. Accesses to SPE registers at
-		 * S-EL1 generate trap exceptions to EL3.
+		 * MDCR_EL2.TPMS (ARM v8.2): Do not trap statistical
+		 * profiling controls to EL2.
+		 *
+		 * MDCR_EL2.E2PB (ARM v8.2): SPE enabled in Non-secure
+		 * state. Accesses to profiling buffer controls at
+		 * Non-secure EL1 are not trapped to EL2.
 		 */
-		v = read_mdcr_el3();
-		v |= MDCR_NSPB(MDCR_NSPB_EL1);
-		write_mdcr_el3(v);
+		v = read_mdcr_el2();
+		v &= ~MDCR_EL2_TPMS;
+		v |= MDCR_EL2_E2PB(MDCR_EL2_E2PB_EL1);
+		write_mdcr_el2(v);
 	}
+
+	/*
+	 * MDCR_EL2.NSPB (ARM v8.2): SPE enabled in Non-secure state
+	 * and disabled in secure state. Accesses to SPE registers at
+	 * S-EL1 generate trap exceptions to EL3.
+	 */
+	v = read_mdcr_el3();
+	v |= MDCR_NSPB(MDCR_NSPB_EL1);
+	write_mdcr_el3(v);
 }
 
 void spe_disable(void)
 {
-	uint64_t features;
+	uint64_t v;
 
-	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_PMS_SHIFT;
-	if ((features & ID_AA64DFR0_PMS_MASK) == 1) {
-		uint64_t v;
+	if (!spe_supported())
+		return;
 
-		/* Drain buffered data */
-		psb_csync();
-		dsbnsh();
+	/* Drain buffered data */
+	psb_csync();
+	dsbnsh();
 
-		/* Disable profiling buffer */
-		v = read_pmblimitr_el1();
-		v &= ~(1ULL << 0);
-		write_pmblimitr_el1(v);
-		isb();
-	}
+	/* Disable profiling buffer */
+	v = read_pmblimitr_el1();
+	v &= ~(1ULL << 0);
+	write_pmblimitr_el1(v);
+	isb();
 }
 
 static void *spe_drain_buffers_hook(const void *arg)
 {
-	uint64_t features;
-
-	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_PMS_SHIFT;
-	if ((features & ID_AA64DFR0_PMS_MASK) == 1) {
-		/* Drain buffered data */
-		psb_csync();
-		dsbnsh();
-	}
+	if (!spe_supported())
+		return (void *)-1;
 
+	/* Drain buffered data */
+	psb_csync();
+	dsbnsh();
 	return 0;
 }
 

lib/extensions/sve/sve.c

 /*
- * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,117 +9,120 @@
 #include <pubsub.h>
 #include <sve.h>
 
-static void *disable_sve_hook(const void *arg)
+int sve_supported(void)
 {
 	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);
+	return (features & ID_AA64PFR0_SVE_MASK) == 1;
+}
 
-		/*
-		 * No explicit ISB required here as ERET to switch to Secure
-		 * world covers it
-		 */
-	}
+static void *disable_sve_hook(const void *arg)
+{
+	uint64_t cptr;
+
+	if (!sve_supported())
+		return (void *)-1;
+
+	/*
+	 * 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
-		 */
-	}
+	uint64_t cptr;
+
+	if (!sve_supported())
+		return (void *)-1;
+
+	/*
+	 * 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;
+	uint64_t cptr;
+
+	if (!sve_supported())
+		return;
 
-	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);
+	/*
+	 * 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_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.
+		 * 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_EL3.EZ: Set to 1 to enable access to SVE  functionality
-		 *  in the Non-secure world.
+		 * CPTR_EL2.TZ: Set to 0 to enable access to SVE support
+		 *  for EL2 and Non-secure EL1 and EL0.
 		 */
-		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();
+		cptr = read_cptr_el2();
+		cptr &= ~(CPTR_EL2_TZ_BIT);
+		write_cptr_el2(cptr);
 
 		/*
 		 * 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.
-		 */
+		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);