@@ -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);
+	u32 os_lsr = etm4x_relaxed_read32(csa, TRCOSLSR);
+
+	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,

@@ -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;