An ETM instance may not implement the OS/Software Lock. This
is advertised via TRCOSLSR/TRCLSR respectively. Detect the
presence of these lock registers and skip the lock/unlock
if they are not implemented.

Signed-off-by: Suzuki K Poulose <suzuki.poulose@arm.com>
---
 drivers/hwtracing/coresight/coresight-etm4x.c | 77 +++++++++++++++----
 drivers/hwtracing/coresight/coresight-etm4x.h | 24 +++++-
 2 files changed, 83 insertions(+), 18 deletions(-)

diff --git a/drivers/hwtracing/coresight/coresight-etm4x.c b/drivers/hwtracing/coresight/coresight-etm4x.c
index 34b27c26591b..7feb57108bdc 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x.c

@@ -95,12 +95,19 @@ void etm4x_sysreg_write(struct csdev_access *csa,
        }
 }

+static inline bool etm4_os_lock_implemented(struct etmv4_drvdata *drvdata)
+{
+       return drvdata->os_lock_model == ETM_OS_LOCK_IMPLEMENTED;
+}
+
 static void etm4_os_unlock_csa(struct etmv4_drvdata *drvdata, struct csdev_access *csa)
 {
-       /* Writing 0 to TRCOSLAR unlocks the trace registers */
-       etm4x_relaxed_write32(csa, 0x0, TRCOSLAR);
+       if (etm4_os_lock_implemented(drvdata)) {
+               /* Writing 0 to TRCOSLAR unlocks the trace registers */
+               etm4x_relaxed_write32(csa, 0x0, TRCOSLAR);
+               isb();
+       }
        drvdata->os_unlock = true;
-       isb();
 }

 static void etm4_os_unlock(struct etmv4_drvdata *drvdata)

@@ -115,10 +122,26 @@ static void etm4_os_lock(struct etmv4_drvdata *drvdata)
        if (WARN_ON(!drvdata->csdev))
                return;

-       /* Writing 0x1 to TRCOSLAR locks the trace registers */
-       etm4x_relaxed_write32(&drvdata->csdev->access, 0x1, TRCOSLAR);
+       if (etm4_os_lock_implemented(drvdata)) {
+               /* Writing 0x1 to TRCOSLAR locks the trace registers */
+               etm4x_relaxed_write32(&drvdata->csdev->access, 0x1, TRCOSLAR);
+               isb();
+       }
        drvdata->os_unlock = false;
-       isb();
+}
+
+static void etm4_cs_lock(struct etmv4_drvdata *drvdata,
+                        struct csdev_access *csa)
+{
+       if (drvdata->sw_lock)
+               CS_LOCK(csa);
+}
+
+static void etm4_cs_unlock(struct etmv4_drvdata *drvdata,
+                        struct csdev_access *csa)
+{
+       if (drvdata->sw_lock)
+               CS_UNLOCK(csa);
 }

 static bool etm4_arch_supported(u8 arch)

@@ -160,7 +183,7 @@ static int etm4_enable_hw(struct etmv4_drvdata *drvdata)
        struct device *etm_dev = &csdev->dev;
        struct csdev_access *csa = &csdev->access;

-       CS_UNLOCK(csa);
+       etm4_cs_unlock(drvdata, csa);

        etm4_os_unlock(drvdata);

@@ -258,7 +281,7 @@ static int etm4_enable_hw(struct etmv4_drvdata *drvdata)
        isb();

 done:
-       CS_LOCK(csa);
+       etm4_cs_lock(drvdata, csa);

        dev_dbg(etm_dev, "cpu: %d enable smp call done: %d\n",
                drvdata->cpu, rc);

@@ -515,7 +538,7 @@ static void etm4_disable_hw(void *info)
        struct csdev_access *csa = &csdev->access;
        int i;

-       CS_UNLOCK(csa);
+       etm4_cs_unlock(drvdata, csa);

        if (!drvdata->skip_power_up) {
                /* power can be removed from the trace unit now */

@@ -557,7 +580,7 @@ static void etm4_disable_hw(void *info)

        coresight_disclaim_device_unlocked(csdev);

-       CS_LOCK(csa);
+       etm4_cs_lock(drvdata, csa);

        dev_dbg(&drvdata->csdev->dev,
                "cpu: %d disable smp call done\n", drvdata->cpu);

@@ -652,6 +675,20 @@ static const struct coresight_ops etm4_cs_ops = {
        .source_ops     = &etm4_source_ops,
 };

+/*
+ * ETM may or may not implement the Software Lock and the OS Lock.
+ * Detect this before we attempt to do any locking.
+ */
+static void etm_detect_lock_status(struct etmv4_drvdata *drvdata,
+                                  struct csdev_access *csa)
+{
+       u32 sw_lsr = etm4x_relaxed_read32(csa, TRCLSR);

+
+       drvdata->sw_lock = TRCLSR_SLI(sw_lsr);
+       drvdata->os_lock_model = TRCOSLSR_OSM(os_lsr);
+}
+
 static void etm4_init_arch_data(void *info)
 {
        u32 etmidr0;

@@ -664,10 +701,15 @@ static void etm4_init_arch_data(void *info)
        int i;
        struct csdev_access csa = CSDEV_ACCESS_IOMEM(drvdata->base);

+       /*
+        * We must check if the locks are implemented
+        * as early as possible.
+        */
+       etm_detect_lock_status(drvdata, &csa);
+
        /* Make sure all registers are accessible */
        etm4_os_unlock_csa(drvdata, &csa);
-
-       CS_UNLOCK(&csa);
+       etm4_cs_unlock(drvdata, &csa);

        /* find all capabilities of the tracing unit */
        etmidr0 = etm4x_relaxed_read32(&csa, TRCIDR0);

@@ -821,7 +863,8 @@ static void etm4_init_arch_data(void *info)
        drvdata->nrseqstate = BMVAL(etmidr5, 25, 27);
        /* NUMCNTR, bits[30:28] number of counters available for tracing */
        drvdata->nr_cntr = BMVAL(etmidr5, 28, 30);
-       CS_LOCK(&csa);
+
+       etm4_cs_lock(drvdata, &csa);
 }

 /* Set ELx trace filter access in the TRCVICTLR register */

@@ -1202,7 +1245,7 @@ static int etm4_cpu_save(struct etmv4_drvdata *drvdata)
        dsb(sy);
        isb();

-       CS_UNLOCK(csa);
+       etm4_cs_unlock(drvdata, csa);

        /* Lock the OS lock to disable trace and external debugger access */
        etm4_os_lock(drvdata);

@@ -1309,7 +1352,7 @@ static int etm4_cpu_save(struct etmv4_drvdata *drvdata)
        etm4x_relaxed_write32(csa, (state->trcpdcr & ~TRCPDCR_PU), TRCPDCR);

 out:
-       CS_LOCK(csa);
+       etm4_cs_lock(drvdata, csa);
        return ret;
 }

@@ -1324,7 +1367,7 @@ static void etm4_cpu_restore(struct etmv4_drvdata *drvdata)
                return;

        csa = &csdev->access;
-       CS_UNLOCK(csa);
+       etm4_cs_unlock(drvdata, csa);

        etm4x_relaxed_write32(csa, state->trcclaimset, TRCCLAIMSET);

@@ -1404,7 +1447,7 @@ static void etm4_cpu_restore(struct etmv4_drvdata *drvdata)

        /* Unlock the OS lock to re-enable trace and external debug access */
        etm4_os_unlock(drvdata);
-       CS_LOCK(csa);
+       etm4_cs_lock(drvdata, csa);
 }

 static int etm4_cpu_pm_notify(struct notifier_block *nb, unsigned long cmd,

diff --git a/drivers/hwtracing/coresight/coresight-etm4x.h b/drivers/hwtracing/coresight/coresight-etm4x.h
index 29ffad6a5279..efd903688edd 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.h
+++ b/drivers/hwtracing/coresight/coresight-etm4x.h

@@ -417,7 +417,7 @@
                if ((csa)->io_mem)                                      \
                        writel_relaxed((val), (csa)->base + (offset));  \
                else                                                    \
-                       write_etm4x_sysreg_offset((csa), (val), \
+                       write_etm4x_sysreg_offset((csa), (val),         \
                                                    (offset), false);   \
        } while (0)

@@ -541,6 +541,22 @@
 /* NS MON (EL3) mode never implemented */
 #define ETM_EXLEVEL_NS_VICTLR_MASK     GENMASK(22, 20)

+/*
+ * TRCOSLSR.OSM - Bit[3,0]
+ *
+ * 0b00        - Trace OS Lock is not implemented.
+ * 0b10        - Trace OS Lock is implemented.
+ */
+#define TRCOSLSR_OSM(x)                        ((((x) & BIT(3)) >> 2) | ((x) & BIT(0)))
+#define ETM_OS_LOCK_IMPLEMENTED                0b10
+

+/*
+ * TRCLSR.SLI - Bit[0]
+ * 0b0 - Software Lock is not implemented.
+ * 0b1 - Software Lock is implmented.
+ */
+#define TRCLSR_SLI(x)                  ((x) & BIT(0))
+
 /**
  * struct etmv4_config - configuration information related to an ETMv4
  * @mode:      Controls various modes supported by this ETM.

@@ -726,6 +742,10 @@ struct etmv4_save_state {
  *             supported for the corresponding Exception level.
  * @ns_ex_level:In non-secure state, indicates whether instruction tracing is
  *             supported for the corresponding Exception level.
+ * @sw_lock:   Cached value of TRCLSR.SLI, tells you whether the software lock
+ *             is implemented.
+ * @os_lock_model: Cached value of TRCOSLSR.OSM, tells you whether OS lock
+ *             is implemented.
  * @sticky_enable: true if ETM base configuration has been done.
  * @boot_enable:True if we should start tracing at boot time.
  * @os_unlock:  True if access to management registers is allowed.

@@ -782,6 +802,8 @@ struct etmv4_drvdata {
        u8                              s_ex_level;
        u8                              ns_ex_level;
        u8                              q_support;
+       u8                              sw_lock;
+       u8                              os_lock_model;
        bool                            sticky_enable;
        bool                            boot_enable;
        bool                            os_unlock;
--

2.24.1