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,14 +14,21 @@
  */
 #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) {
+	return (features & ID_AA64DFR0_PMS_MASK) == 1;
+}
+
+void spe_enable(int el2_unused)
+{
 	uint64_t v;
 
+	if (!spe_supported())
+		return;
+
 	if (el2_unused) {
 		/*
 		 * MDCR_EL2.TPMS (ARM v8.2): Do not trap statistical
@@ -45,17 +52,15 @@ void spe_enable(int el2_unused)
 	v = read_mdcr_el3();
 	v |= MDCR_NSPB(MDCR_NSPB_EL1);
 	write_mdcr_el3(v);
-	}
 }
 
 void spe_disable(void)
 {
-	uint64_t features;
-
-	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();
@@ -65,20 +70,16 @@ void spe_disable(void)
 	v &= ~(1ULL << 0);
 	write_pmblimitr_el1(v);
 	isb();
-	}
 }
 
 static void *spe_drain_buffers_hook(const void *arg)
 {
-	uint64_t features;
+	if (!spe_supported())
+		return (void *)-1;
 
-	features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_PMS_SHIFT;
-	if ((features & ID_AA64DFR0_PMS_MASK) == 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,14 +9,21 @@
 #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) {
+	return (features & ID_AA64PFR0_SVE_MASK) == 1;
+}
+
+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
@@ -32,18 +39,16 @@ static void *disable_sve_hook(const void *arg)
 	 * 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;
 
+	if (!sve_supported())
+		return (void *)-1;
+
 	/*
 	 * Enable SVE, SIMD and FP access for the Non-secure world.
 	 */
@@ -55,17 +60,16 @@ static void *enable_sve_hook(const void *arg)
 	 * 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 (!sve_supported())
+		return;
+
 #if CTX_INCLUDE_FPREGS
 	/*
 	 * CTX_INCLUDE_FPREGS is not supported on SVE enabled systems.
@@ -119,7 +123,6 @@ void sve_enable(int el2_unused)
 	 * 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);