---
.../coresight/coresight-etm4x-core.c | 126 ++++++++++--------
1 file changed, 74 insertions(+), 52 deletions(-)
diff --git a/drivers/hwtracing/coresight/coresight-etm4x-core.c b/drivers/hwtracing/coresight/coresight-etm4x-core.c
index 2c1a60577728..71be566bd117 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x-core.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x-core.c
@@ -428,6 +428,78 @@ static void etm4_check_arch_features(struct etmv4_drvdata *drvdata,
}
#endif /* CONFIG_ETM4X_IMPDEF_FEATURE */
+static int etm4_enable_trace_unit(struct etmv4_drvdata *drvdata)
+{
+ struct coresight_device *csdev = drvdata->csdev;
+ struct device *etm_dev = &csdev->dev;
+ struct csdev_access *csa = &csdev->access;
+
+ /*
+ * ETE mandates that the TRCRSR is written to before
+ * enabling it.
+ */
+ if (etm4x_is_ete(drvdata))
+ etm4x_relaxed_write32(csa, TRCRSR_TA, TRCRSR);
+
+ etm4x_allow_trace(drvdata);
+ /* Enable the trace unit */
+ etm4x_relaxed_write32(csa, 1, TRCPRGCTLR);
+
+ /* Synchronize the register updates for sysreg access */
+ if (!csa->io_mem)
+ isb();
+
+ /* wait for TRCSTATR.IDLE to go back down to '0' */
+ if (coresight_timeout(csa, TRCSTATR, TRCSTATR_IDLE_BIT, 0)) {
+ dev_err(etm_dev,
+ "timeout while waiting for Idle Trace Status\n");
+ return -ETIME;
+ }
+
+ /*
+ * As recommended by section 4.3.7 ("Synchronization when using the
+ * memory-mapped interface") of ARM IHI 0064D
+ */
+ dsb(sy);
+ isb();
+
+ return 0;
+}
+
+static void etm4_disable_trace_unit(struct etmv4_drvdata *drvdata)
+{
+ u32 control;
+ struct coresight_device *csdev = drvdata->csdev;
+ struct device *etm_dev = &csdev->dev;
+ struct csdev_access *csa = &csdev->access;
+
+ control = etm4x_relaxed_read32(csa, TRCPRGCTLR);
+
+ /* EN, bit[0] Trace unit enable bit */
+ control &= ~0x1;
+
+ /*
+ * If the CPU supports v8.4 Trace filter Control,
+ * set the ETM to trace prohibited region.
+ */
+ etm4x_prohibit_trace(drvdata);
+ /*
+ * Make sure everything completes before disabling, as recommended
+ * by section 7.3.77 ("TRCVICTLR, ViewInst Main Control Register,
+ * SSTATUS") of ARM IHI 0064D
+ */
+ dsb(sy);
+ isb();
+ /* Trace synchronization barrier, is a nop if not supported */
+ tsb_csync();
+ etm4x_relaxed_write32(csa, control, TRCPRGCTLR);
+
+ /* wait for TRCSTATR.PMSTABLE to go to '1' */
+ if (coresight_timeout(csa, TRCSTATR, TRCSTATR_PMSTABLE_BIT, 1))
+ dev_err(etm_dev,
+ "timeout while waiting for PM stable Trace Status\n");
+}
+
static int etm4_enable_hw(struct etmv4_drvdata *drvdata)
{
int i, rc;
@@ -536,33 +608,7 @@ static int etm4_enable_hw(struct etmv4_drvdata *drvdata)
etm4x_relaxed_write32(csa, trcpdcr | TRCPDCR_PU, TRCPDCR);
}
- /*
- * ETE mandates that the TRCRSR is written to before
- * enabling it.
- */
- if (etm4x_is_ete(drvdata))
- etm4x_relaxed_write32(csa, TRCRSR_TA, TRCRSR);
-
- etm4x_allow_trace(drvdata);
- /* Enable the trace unit */
- etm4x_relaxed_write32(csa, 1, TRCPRGCTLR);
-
- /* Synchronize the register updates for sysreg access */
- if (!csa->io_mem)
- isb();
-
- /* wait for TRCSTATR.IDLE to go back down to '0' */
- if (coresight_timeout(csa, TRCSTATR, TRCSTATR_IDLE_BIT, 0))
- dev_err(etm_dev,
- "timeout while waiting for Idle Trace Status\n");
-
- /*
- * As recommended by section 4.3.7 ("Synchronization when using the
- * memory-mapped interface") of ARM IHI 0064D
- */
- dsb(sy);
- isb();
-
+ rc = etm4_enable_trace_unit(drvdata);
done:
etm4_cs_lock(drvdata, csa);
@@ -906,7 +952,6 @@ static void etm4_disable_hw(void *info)
struct etmv4_drvdata *drvdata = info;
struct etmv4_config *config = &drvdata->config;
struct coresight_device *csdev = drvdata->csdev;
- struct device *etm_dev = &csdev->dev;
struct csdev_access *csa = &csdev->access;
int i;
@@ -920,31 +965,8 @@ static void etm4_disable_hw(void *info)
etm4x_relaxed_write32(csa, control, TRCPDCR);
}
- control = etm4x_relaxed_read32(csa, TRCPRGCTLR);
+ etm4_disable_trace_unit(drvdata);
- /* EN, bit[0] Trace unit enable bit */
- control &= ~0x1;
-
- /*
- * If the CPU supports v8.4 Trace filter Control,
- * set the ETM to trace prohibited region.
- */
- etm4x_prohibit_trace(drvdata);
- /*
- * Make sure everything completes before disabling, as recommended
- * by section 7.3.77 ("TRCVICTLR, ViewInst Main Control Register,
- * SSTATUS") of ARM IHI 0064D
- */
- dsb(sy);
- isb();
- /* Trace synchronization barrier, is a nop if not supported */
- tsb_csync();
- etm4x_relaxed_write32(csa, control, TRCPRGCTLR);
-
- /* wait for TRCSTATR.PMSTABLE to go to '1' */
- if (coresight_timeout(csa, TRCSTATR, TRCSTATR_PMSTABLE_BIT, 1))
- dev_err(etm_dev,
- "timeout while waiting for PM stable Trace Status\n");
/* read the status of the single shot comparators */
for (i = 0; i < drvdata->nr_ss_cmp; i++) {
config->ss_status[i] =