From 2e94da13790336eb3fd00fb5e97610dd9aebe213 Mon Sep 17 00:00:00 2001
From: Will Deacon <will.deacon@arm.com>
Date: Mon, 27 Jul 2015 16:23:58 +0100
Subject: [PATCH] arm64: lse: use generic cpufeature detection for LSE atomics

Rework the cpufeature detection to support ISAR0 and use that for
detecting the presence of LSE atomics.

Signed-off-by: Will Deacon <will.deacon@arm.com>
---
 arch/arm64/kernel/cpufeature.c | 37 +++++++++++++++++++---------------
 arch/arm64/kernel/setup.c      |  4 ----
 2 files changed, 21 insertions(+), 20 deletions(-)

diff --git a/arch/arm64/kernel/cpufeature.c b/arch/arm64/kernel/cpufeature.c
index 978fa169d3c3..3c9aed32f70b 100644
--- a/arch/arm64/kernel/cpufeature.c
+++ b/arch/arm64/kernel/cpufeature.c
@@ -31,23 +31,19 @@ feature_matches(u64 reg, const struct arm64_cpu_capabilities *entry)
 	return val >= entry->min_field_value;
 }
 
-static bool
-has_id_aa64pfr0_feature(const struct arm64_cpu_capabilities *entry)
-{
-	u64 val;
-
-	val = read_cpuid(id_aa64pfr0_el1);
-	return feature_matches(val, entry);
+#define __ID_FEAT_CHK(reg)						\
+static bool __maybe_unused						\
+has_##reg##_feature(const struct arm64_cpu_capabilities *entry)		\
+{									\
+	u64 val;							\
+									\
+	val = read_cpuid(reg##_el1);					\
+	return feature_matches(val, entry);				\
 }
 
-static bool __maybe_unused
-has_id_aa64mmfr1_feature(const struct arm64_cpu_capabilities *entry)
-{
-	u64 val;
-
-	val = read_cpuid(id_aa64mmfr1_el1);
-	return feature_matches(val, entry);
-}
+__ID_FEAT_CHK(id_aa64pfr0);
+__ID_FEAT_CHK(id_aa64mmfr1);
+__ID_FEAT_CHK(id_aa64isar0);
 
 static const struct arm64_cpu_capabilities arm64_features[] = {
 	{
@@ -67,6 +63,15 @@ static const struct arm64_cpu_capabilities arm64_features[] = {
 		.enable = cpu_enable_pan,
 	},
 #endif /* CONFIG_ARM64_PAN */
+#if defined(CONFIG_AS_LSE) && defined(CONFIG_ARM64_LSE_ATOMICS)
+	{
+		.desc = "LSE atomic instructions",
+		.capability = ARM64_HAS_LSE_ATOMICS,
+		.matches = has_id_aa64isar0_feature,
+		.field_pos = 20,
+		.min_field_value = 2,
+	},
+#endif /* CONFIG_AS_LSE && CONFIG_ARM64_LSE_ATOMICS */
 	{},
 };
 
@@ -93,5 +98,5 @@ void check_cpu_capabilities(const struct arm64_cpu_capabilities *caps,
 
 void check_local_cpu_features(void)
 {
-	check_cpu_capabilities(arm64_features, "detected feature");
+	check_cpu_capabilities(arm64_features, "detected feature:");
 }
diff --git a/arch/arm64/kernel/setup.c b/arch/arm64/kernel/setup.c
index 82ae8429baf2..b2f9895ecf7b 100644
--- a/arch/arm64/kernel/setup.c
+++ b/arch/arm64/kernel/setup.c
@@ -284,10 +284,6 @@ static void __init setup_processor(void)
 		default:
 		case 2:
 			elf_hwcap |= HWCAP_ATOMICS;
-			cpus_set_cap(ARM64_HAS_LSE_ATOMICS);
-			if (IS_ENABLED(CONFIG_AS_LSE) &&
-			    IS_ENABLED(CONFIG_ARM64_LSE_ATOMICS))
-				pr_info("LSE atomics supported\n");
 		case 1:
 			/* RESERVED */
 		case 0: