diff --git a/drivers/hwtracing/coresight/coresight-tmc-etr.c b/drivers/hwtracing/coresight/coresight-tmc-etr.c
index be77101e2b1b..54d8973a8a4b 100644
--- a/drivers/hwtracing/coresight/coresight-tmc-etr.c
+++ b/drivers/hwtracing/coresight/coresight-tmc-etr.c
@@ -1404,7 +1404,6 @@ static int tmc_enable_etr_sink_sysfs(struct coresight_device *csdev)
 	 * buffer, provided the size matches. Any allocation has to be done
 	 * with the lock released.
 	 */
-	mutex_lock(&drvdata->mem_lock);
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 	sysfs_buf = READ_ONCE(drvdata->sysfs_buf);
 	if (!sysfs_buf || (sysfs_buf->size != drvdata->size)) {
@@ -1422,8 +1421,7 @@ static int tmc_enable_etr_sink_sysfs(struct coresight_device *csdev)
 			/* Allocate memory with the locks released */
 			free_buf = new_buf = tmc_etr_setup_sysfs_buf(drvdata);
 			if (IS_ERR(new_buf)) {
-				mutex_unlock(&drvdata->mem_lock);
-				return PTR_ERR(new_buf);
+				return -ENOMEM;
 			}
 			coresight_cti_map_trigout(drvdata->cti_flush, 3, 0);
 			coresight_cti_map_trigin(drvdata->cti_reset, 2, 0);
@@ -1431,11 +1429,8 @@ static int tmc_enable_etr_sink_sysfs(struct coresight_device *csdev)
 			drvdata->usbch = usb_qdss_open("qdss", drvdata,
 								usb_notifier);
 			if (IS_ERR_OR_NULL(drvdata->usbch)) {
-				dev_err(&drvdata->csdev->dev,
-					"usb_qdss_open failed\n");
-				ret = PTR_ERR(drvdata->usbch);
-				mutex_unlock(&drvdata->mem_lock);
-				return ret;
+				dev_err(&csdev->dev, "usb_qdss_open failed\n");
+				return -ENODEV;
 			}
 		}
 		spin_lock_irqsave(&drvdata->spinlock, flags);
@@ -1483,8 +1478,8 @@ out:
 	if (free_buf)
 		tmc_etr_free_sysfs_buf(free_buf);
 
-	tmc_etr_byte_cntr_start(drvdata->byte_cntr);
-	mutex_unlock(&drvdata->mem_lock);
+	if (drvdata->out_mode == TMC_ETR_OUT_MODE_MEM)
+		tmc_etr_byte_cntr_start(drvdata->byte_cntr);
 
 	if (!ret)
 		dev_dbg(&csdev->dev, "TMC-ETR enabled\n");
@@ -1914,34 +1909,37 @@ unlock_out:
 static int tmc_enable_etr_sink(struct coresight_device *csdev,
 			       u32 mode, void *data)
 {
+	struct tmc_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	int ret;
+
 	switch (mode) {
 	case CS_MODE_SYSFS:
-		return tmc_enable_etr_sink_sysfs(csdev);
+		mutex_lock(&drvdata->mem_lock);
+		ret = tmc_enable_etr_sink_sysfs(csdev);
+		mutex_unlock(&drvdata->mem_lock);
+		return ret;
+
 	case CS_MODE_PERF:
 		return tmc_enable_etr_sink_perf(csdev, data);
 	}
-
 	/* We shouldn't be here */
 	return -EINVAL;
 }
 
-static int tmc_disable_etr_sink(struct coresight_device *csdev)
+static int _tmc_disable_etr_sink(struct coresight_device *csdev)
 {
 	unsigned long flags;
 	struct tmc_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
 
-	mutex_lock(&drvdata->mem_lock);
 	spin_lock_irqsave(&drvdata->spinlock, flags);
 
 	if (drvdata->reading) {
 		spin_unlock_irqrestore(&drvdata->spinlock, flags);
-		mutex_unlock(&drvdata->mem_lock);
 		return -EBUSY;
 	}
 
 	if (atomic_dec_return(csdev->refcnt)) {
 		spin_unlock_irqrestore(&drvdata->spinlock, flags);
-		mutex_unlock(&drvdata->mem_lock);
 		return -EBUSY;
 	}
 
@@ -1977,11 +1975,64 @@ static int tmc_disable_etr_sink(struct coresight_device *csdev)
 		drvdata->etr_buf = NULL;
 	}
 out:
-	mutex_unlock(&drvdata->mem_lock);
 	dev_info(&csdev->dev, "TMC-ETR disabled\n");
 	return 0;
 }
 
+static int tmc_disable_etr_sink(struct coresight_device *csdev)
+{
+	struct tmc_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
+	int ret;
+
+	mutex_lock(&drvdata->mem_lock);
+	ret = _tmc_disable_etr_sink(csdev);
+	mutex_unlock(&drvdata->mem_lock);
+	return ret;
+}
+
+int tmc_etr_switch_mode(struct tmc_drvdata *drvdata, const char *out_mode)
+{
+	enum tmc_etr_out_mode new_mode, old_mode;
+
+	mutex_lock(&drvdata->mem_lock);
+	if (!strcmp(out_mode, str_tmc_etr_out_mode[TMC_ETR_OUT_MODE_MEM]))
+		new_mode = TMC_ETR_OUT_MODE_MEM;
+	else if (!strcmp(out_mode, str_tmc_etr_out_mode[TMC_ETR_OUT_MODE_USB]))
+		new_mode = TMC_ETR_OUT_MODE_USB;
+	else {
+		mutex_unlock(&drvdata->mem_lock);
+		return -EINVAL;
+	}
+
+	if (new_mode == drvdata->out_mode) {
+		mutex_unlock(&drvdata->mem_lock);
+		return 0;
+	}
+
+	if (drvdata->mode == CS_MODE_DISABLED) {
+		drvdata->out_mode = new_mode;
+		mutex_unlock(&drvdata->mem_lock);
+		return 0;
+	}
+
+	tmc_disable_etr_sink(drvdata->csdev);
+	old_mode = drvdata->out_mode;
+	drvdata->out_mode = new_mode;
+	if (tmc_enable_etr_sink_sysfs(drvdata->csdev)) {
+		drvdata->out_mode = old_mode;
+		tmc_enable_etr_sink_sysfs(drvdata->csdev);
+		dev_err(&drvdata->csdev->dev,
+			"Switch to %s failed. Fall back to %s.\n",
+			str_tmc_etr_out_mode[new_mode],
+			str_tmc_etr_out_mode[old_mode]);
+		mutex_unlock(&drvdata->mem_lock);
+		return -EINVAL;
+	}
+
+	mutex_unlock(&drvdata->mem_lock);
+	return 0;
+}
+
 static const struct coresight_ops_sink tmc_etr_sink_ops = {
 	.enable		= tmc_enable_etr_sink,
 	.disable	= tmc_disable_etr_sink,
diff --git a/drivers/hwtracing/coresight/coresight-tmc.c b/drivers/hwtracing/coresight/coresight-tmc.c
index 0023d1649a8a..169d044ee8c1 100644
--- a/drivers/hwtracing/coresight/coresight-tmc.c
+++ b/drivers/hwtracing/coresight/coresight-tmc.c
@@ -416,77 +416,14 @@ static ssize_t out_mode_store(struct device *dev,
 {
 	struct tmc_drvdata *drvdata = dev_get_drvdata(dev->parent);
 	char str[10] = "";
-	unsigned long flags;
 	int ret;
 
 	if (strlen(buf) >= 10)
 		return -EINVAL;
 	if (sscanf(buf, "%10s", str) != 1)
 		return -EINVAL;
-
-	mutex_lock(&drvdata->mem_lock);
-	if (!strcmp(str, str_tmc_etr_out_mode[TMC_ETR_OUT_MODE_MEM])) {
-		if (drvdata->out_mode == TMC_ETR_OUT_MODE_MEM)
-			goto out;
-
-		spin_lock_irqsave(&drvdata->spinlock, flags);
-		if (!drvdata->enable) {
-			drvdata->out_mode = TMC_ETR_OUT_MODE_MEM;
-			spin_unlock_irqrestore(&drvdata->spinlock, flags);
-			goto out;
-		}
-		__tmc_etr_disable_to_bam(drvdata);
-		tmc_etr_enable_hw(drvdata, drvdata->sysfs_buf);
-		drvdata->out_mode = TMC_ETR_OUT_MODE_MEM;
-		spin_unlock_irqrestore(&drvdata->spinlock, flags);
-
-		coresight_cti_map_trigout(drvdata->cti_flush, 3, 0);
-		coresight_cti_map_trigin(drvdata->cti_reset, 2, 0);
-
-		tmc_etr_bam_disable(drvdata);
-		usb_qdss_close(drvdata->usbch);
-	} else if (!strcmp(str, str_tmc_etr_out_mode[TMC_ETR_OUT_MODE_USB])) {
-		if (drvdata->out_mode == TMC_ETR_OUT_MODE_USB)
-			goto out;
-
-		spin_lock_irqsave(&drvdata->spinlock, flags);
-		if (!drvdata->enable) {
-			drvdata->out_mode = TMC_ETR_OUT_MODE_USB;
-			spin_unlock_irqrestore(&drvdata->spinlock, flags);
-			goto out;
-		}
-		if (drvdata->reading) {
-			ret = -EBUSY;
-			goto err1;
-		}
-		tmc_etr_disable_hw(drvdata);
-		drvdata->out_mode = TMC_ETR_OUT_MODE_USB;
-		spin_unlock_irqrestore(&drvdata->spinlock, flags);
-
-		if (drvdata->mode != CS_MODE_DISABLED) {
-			coresight_cti_unmap_trigin(drvdata->cti_reset, 2, 0);
-			coresight_cti_unmap_trigout(drvdata->cti_flush, 3, 0);
-			tmc_etr_byte_cntr_stop(drvdata->byte_cntr);
-			if (drvdata->etr_buf)
-				tmc_free_etr_buf(drvdata->etr_buf);
-		}
-
-		drvdata->usbch = usb_qdss_open("qdss", drvdata,
-					       usb_notifier);
-		if (IS_ERR(drvdata->usbch)) {
-			dev_err(dev, "usb_qdss_open failed\n");
-			ret = PTR_ERR(drvdata->usbch);
-			goto err0;
-		}
-	}
-out:
-	mutex_unlock(&drvdata->mem_lock);
-	return size;
-err1:
-	spin_unlock_irqrestore(&drvdata->spinlock, flags);
-err0:
-	mutex_unlock(&drvdata->mem_lock);
-	return ret;
+	ret = tmc_etr_switch_mode(drvdata, str);
+	return ret ? ret : size;
 }
 static DEVICE_ATTR_RW(out_mode);
 
@@ -508,7 +445,6 @@ static DEVICE_ATTR_RO(available_out_modes);
 
 static struct attribute *coresight_tmc_etf_attrs[] = {
 	&dev_attr_trigger_cntr.attr,
-	&dev_attr_buffer_size.attr,
 	NULL,
 };
 
diff --git a/drivers/hwtracing/coresight/coresight-tmc.h b/drivers/hwtracing/coresight/coresight-tmc.h
index 736431b5ee33..b9543048aafc 100644
--- a/drivers/hwtracing/coresight/coresight-tmc.h
+++ b/drivers/hwtracing/coresight/coresight-tmc.h
@@ -338,6 +338,8 @@ ssize_t tmc_etr_get_sysfs_trace(struct tmc_drvdata *drvdata,
 				loff_t pos, size_t len, char **bufpp);
 ssize_t tmc_etr_buf_get_data(struct etr_buf *etr_buf,
 				u64 offset, size_t len, char **bufpp);
+int tmc_etr_switch_mode(struct tmc_drvdata *drvdata, const char *out_mode);
+
 
 #define TMC_REG_PAIR(name, lo_off, hi_off)				\
 static inline u64							\