[PATCH 16/19] coresight: etm4x: Detect system instructions support

[Suzuki K Poulose]

ETM v4.4 onwards adds support for system instruction access
to the ETM. Detect the support on an ETM and switch to using the
mode when available.

Signed-off-by: Suzuki K Poulose <suzuki.poulose at arm.com>
---
 drivers/hwtracing/coresight/coresight-etm4x.c | 31 +++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/drivers/hwtracing/coresight/coresight-etm4x.c b/drivers/hwtracing/coresight/coresight-etm4x.c
index 0fce9fb12cff..dc5ac171db35 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x.c
@@ -693,11 +693,39 @@ static void etm_detect_lock_status(struct etmv4_drvdata *drvdata,
 	drvdata->os_lock_model = TRCOSLSR_OSM(os_lsr);
 }
 
+static inline bool cpu_supports_sysreg_trace(void)
+{
+	u64 dfr0 = read_sysreg_s(SYS_ID_AA64DFR0_EL1);
+
+	return ((dfr0 >> ID_AA64DFR0_TRACEVER_SHIFT) & 0xfUL) > 0;
+}
+
 static inline bool trace_unit_supported(u32 devarch)
 {
 	return (devarch & ETM_DEVARCH_ID_MASK) == ETM_DEVARCH_ETMv4x_ARCH;
 }
 
+static bool etm_init_sysreg_access(struct etmv4_drvdata *drvdata,
+				   struct csdev_access *csa)
+{
+	u32 devarch;
+
+	if (!cpu_supports_sysreg_trace())
+		return false;
+
+	devarch = read_etm4x_sysreg_const_offset(TRCDEVARCH);
+	if (!trace_unit_supported(devarch))
+		return false;
+	*csa = (struct csdev_access) {
+		.io_mem	= false,
+		.read	= etm4x_sysreg_read,
+		.write	= etm4x_sysreg_write,
+	};
+
+	drvdata->arch = devarch;
+	return true;
+}
+
 static bool etm_init_iomem_access(struct etmv4_drvdata *drvdata,
 				  struct csdev_access *csa)
 {
@@ -716,6 +744,9 @@ static bool etm_init_iomem_access(struct etmv4_drvdata *drvdata,
 static bool etm_init_csdev_access(struct etmv4_drvdata *drvdata,
 				  struct csdev_access *csa)
 {
+	if (etm_init_sysreg_access(drvdata, csa))
+		return true;
+
 	if (drvdata->base)
 		return etm_init_iomem_access(drvdata, csa);
 
-- 
2.24.1