[RFC PATCH 11/14] coresight: etm4x: Detect system register access support

[Suzuki K Poulose]

Check if the etm4x supports system register access and use
it instead of the memory mapped IO.

We need to detect that :
 a) The CPU implements system register access to the Trace unit
	AND
 b) The trace unit is an ETMv4.x component.

Cc: Mathieu Poirier <mathieu.poirier at linaro.org>
Cc: Mike Leach <mike.leach at linaro.org>
Signed-off-by: Suzuki K Poulose <suzuki.poulose at arm.com>
---
 drivers/hwtracing/coresight/coresight-etm4x.c | 76 +++++++++++++++----
 drivers/hwtracing/coresight/coresight-etm4x.h | 16 ++++
 2 files changed, 78 insertions(+), 14 deletions(-)

diff --git a/drivers/hwtracing/coresight/coresight-etm4x.c b/drivers/hwtracing/coresight/coresight-etm4x.c
index 776a59f62710..831206f7f306 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x.c
@@ -19,6 +19,7 @@
 #include <linux/clk.h>
 #include <linux/cpu.h>
 #include <linux/cpu_pm.h>
+#include <linux/cpumask.h>
 #include <linux/coresight.h>
 #include <linux/coresight-pmu.h>
 #include <linux/pm_wakeup.h>
@@ -1507,9 +1508,43 @@ static void etm4_pm_clear(void)
 	}
 }
 
+static inline bool trace_unit_is_etmv4(u32 devarch)
+{
+	return (devarch & ETM_DEVARCH_ID_MASK) == ETM_DEVARCH_ETMv4x_ARCH;
+}
+
+static void etm4_check_sysreg_access(void *arg)
+{
+	bool *status = arg;
+	u32 devarch;
+
+	if (!arch_cpu_supports_sysreg_trace()) {
+		*status = false;
+		return;
+	}
+
+	devarch = read_etm4x_sysreg_const_offset(TRCDEVARCH);
+	if (!trace_unit_is_etmv4(devarch)) {
+		*status = false;
+		return;
+	}
+
+	*status = true;
+}
+
+static bool etm4_cpu_supports_sysreg(int cpu)
+{
+	bool sys_reg_support = false;
+
+	smp_call_function_single(cpu, etm4_check_sysreg_access, &sys_reg_support, 1);
+
+	return sys_reg_support;
+}
+
 static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
 {
-	int ret;
+	int ret, cpu;
+	bool sys_reg = false;
 	void __iomem *base;
 	struct device *dev = &adev->dev;
 	struct coresight_platform_data *pdata = NULL;
@@ -1517,10 +1552,17 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
 	struct resource *res = &adev->res;
 	struct coresight_desc desc = { 0 };
 
+	cpu = coresight_get_cpu(dev);
+	if (cpu < 0 || !cpu_online(cpu))
+		return -EPROBE_DEFER;
+
+	sys_reg = etm4_cpu_supports_sysreg(cpu);
+
 	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
 	if (!drvdata)
 		return -ENOMEM;
 
+	drvdata->cpu = cpu;
 	dev_set_drvdata(dev, drvdata);
 
 	if (pm_save_enable == PARAM_PM_SAVE_FIRMWARE)
@@ -1537,26 +1579,30 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
 	if (fwnode_property_present(dev_fwnode(dev), "qcom,skip-power-up"))
 		drvdata->skip_power_up = true;
 
-	/* Validity for the resource is already checked by the AMBA core */
-	base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(base))
-		return PTR_ERR(base);
-
-	drvdata->base = base;
-	desc.access.base = base;
+	if (sys_reg) {
+		desc.access = (struct csdev_access){
+			.no_iomem = true,
+			.read = etm4x_sysreg_read,
+			.write = etm4x_sysreg_write,
+		};
+	} else {
+		/* Validity for the resource is already checked by the AMBA core */
+		base = devm_ioremap_resource(dev, res);
+		if (IS_ERR(base))
+			return PTR_ERR(base);
+
+		drvdata->base = base;
+		desc.access.base = base;
+	}
 
 	spin_lock_init(&drvdata->spinlock);
 
-	drvdata->cpu = coresight_get_cpu(dev);
-	if (drvdata->cpu < 0)
-		return drvdata->cpu;
-
 	desc.name = devm_kasprintf(dev, GFP_KERNEL, "etm%d", drvdata->cpu);
 	if (!desc.name)
 		return -ENOMEM;
 
 	cpus_read_lock();
-	etmdrvdata[drvdata->cpu] = drvdata;
+	etmdrvdata[cpu] = drvdata;
 
 	if (smp_call_function_single(drvdata->cpu,
 				etm4_init_arch_data,  &desc.access, 1))
@@ -1607,6 +1653,8 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
 	pm_runtime_put(&adev->dev);
 	dev_info(&drvdata->csdev->dev, "CPU%d: ETM v%d.%d initialized\n",
 		 drvdata->cpu, drvdata->arch >> 4, drvdata->arch & 0xf);
+	if (sys_reg)
+		dev_info(&drvdata->csdev->dev, "Using system register accesses\n");
 
 	if (boot_enable) {
 		coresight_enable(drvdata->csdev);
@@ -1616,7 +1664,7 @@ static int etm4_probe(struct amba_device *adev, const struct amba_id *id)
 	return 0;
 
 err_arch_supported:
-	etmdrvdata[drvdata->cpu] = NULL;
+	etmdrvdata[cpu] = NULL;
 	etm4_pm_clear();
 	return ret;
 }
diff --git a/drivers/hwtracing/coresight/coresight-etm4x.h b/drivers/hwtracing/coresight/coresight-etm4x.h
index ab3d1c195387..86fdcbafc895 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.h
+++ b/drivers/hwtracing/coresight/coresight-etm4x.h
@@ -152,6 +152,13 @@
 #define write_etm4x_sysreg_const_offset(val, offset)		\
 	WRITE_ETM4x_REG(val, ETM4x_OFFSET_TO_REG(offset))
 
+static inline bool arch_cpu_supports_sysreg_trace(void)
+{
+	u64 dfr0 = read_sysreg_s(SYS_ID_AA64DFR0_EL1);
+
+	return ((dfr0 >> ID_AA64DFR0_TRACEVER_SHIFT) & 0xfUL) > 0;
+}
+
 #elif defined(CONFIG_ARM)
 
 #include <asm/hardware/cp14.h>
@@ -172,6 +179,15 @@
 #define write_etm4x_sysreg_const_offset(val, offset)		\
 	WRITE_ETM4x_REG(val, ETM4x_OFFSET_TO_REG(offset))
 
+#define ID_DFR0_CopTrc_SHIFT	12
+static inline bool arch_cpu_supports_sysreg_trace(void)
+{
+	u32 dfr0;
+
+	asm volatile("mrc p15, 0, %0, c0, c1, 2" : "=r" (dfr0));
+
+	return ((dfr0 >> ID_DFR0_CopTrc_SHIFT) & 0xfUL) > 0;
+}
 #endif
 
 #define CASE_READ(res, x)					\
-- 
2.24.1