From 534fd1b6c49c8378924a2be9a94d4dd68004de9b Mon Sep 17 00:00:00 2001
From: Shashank Mittal <mittals@codeaurora.org>
Date: Tue, 15 Mar 2016 21:42:16 -0700
Subject: [PATCH] coresight-tmc: add support to switch sinks

Add support to switch to a different Coresight sink when one or
more Coresight tracing sources are enabled.

Change-Id: I79f769f0913124710ae56fddea7d205359e09b43
Signed-off-by: Shashank Mittal <mittals@codeaurora.org>
---
 drivers/hwtracing/coresight/coresight.c | 107 +++++++++++++++++++++++-
 1 file changed, 103 insertions(+), 4 deletions(-)

diff --git a/drivers/hwtracing/coresight/coresight.c b/drivers/hwtracing/coresight/coresight.c
index a16871b36048..2a5d1fb48dba 100644
--- a/drivers/hwtracing/coresight/coresight.c
+++ b/drivers/hwtracing/coresight/coresight.c
@@ -28,6 +28,7 @@
 #include "coresight-priv.h"
 
 static DEFINE_MUTEX(coresight_mutex);
+static struct coresight_device *curr_sink;
 
 static int coresight_id_match(struct device *dev, void *data)
 {
@@ -382,6 +383,107 @@ out:
 }
 EXPORT_SYMBOL_GPL(coresight_disable);
 
+static int coresight_disable_all_source(struct device *dev, void *data)
+{
+	struct coresight_device *csdev;
+	LIST_HEAD(path);
+
+	csdev = to_coresight_device(dev);
+
+	/*
+	 * No need to care about components that are not sources or not enabled
+	 */
+	if (!csdev->enable || csdev->type != CORESIGHT_DEV_TYPE_SOURCE)
+		return 0;
+
+	coresight_disable_source(csdev);
+
+	return 0;
+}
+
+static int coresight_toggle_source_path(struct device *dev, void *data)
+{
+	struct coresight_device *csdev;
+	bool *enable = data;
+	int ret;
+	LIST_HEAD(path);
+
+	csdev = to_coresight_device(dev);
+
+	/*
+	 * No need to care about components that are not sources or not enabled
+	 */
+	if (!csdev->enable || csdev->type != CORESIGHT_DEV_TYPE_SOURCE)
+		return 0;
+
+	if (*enable) {
+		ret = coresight_build_paths(csdev, &path, true);
+		if (ret) {
+			dev_err(&csdev->dev, "building path(s) failed\n");
+			return ret;
+		}
+	} else {
+		if (coresight_build_paths(csdev, &path, false))
+			dev_err(&csdev->dev, "releasing path(s) failed\n");
+	}
+
+	return 0;
+}
+
+static int coresight_switch_sink(struct coresight_device *csdev)
+{
+	int ret;
+	LIST_HEAD(slist);
+	bool enable = false;
+
+	mutex_lock(&coresight_mutex);
+
+	/* If curr_sink is same as new requested sink then do nothing. */
+	if (curr_sink == csdev)
+		goto out;
+
+	/*
+	 * If curr_sink is NULL then sink is getting set for the first time.
+	 * No source should be enabled at this time.
+	 */
+	if (!curr_sink) {
+		csdev->activated = true;
+		goto out;
+	}
+
+	/* curr_sink is different from csdev */
+	bus_for_each_dev(&coresight_bustype, NULL,
+			 &enable, coresight_toggle_source_path);
+
+	csdev->activated = true;
+	curr_sink->activated = false;
+
+	enable = true;
+	ret = bus_for_each_dev(&coresight_bustype, NULL, &enable,
+			       coresight_toggle_source_path);
+	if (ret)
+		goto err;
+out:
+	curr_sink = csdev;
+	mutex_unlock(&coresight_mutex);
+	return 0;
+
+err:
+	/* Disable sources */
+	bus_for_each_dev(&coresight_bustype, NULL,
+			 &enable, coresight_disable_all_source);
+
+	enable = false;
+	bus_for_each_dev(&coresight_bustype, NULL,
+			 &enable, coresight_toggle_source_path);
+
+	csdev->activated = false;
+	curr_sink->activated = true;
+
+	mutex_unlock(&coresight_mutex);
+	return ret;
+}
+
 static ssize_t curr_sink_show(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
@@ -403,12 +505,9 @@ static ssize_t curr_sink_store(struct device *dev,
 		return ret;
 
 	if (val)
-		csdev->activated = true;
-	else
-		csdev->activated = false;
+		coresight_switch_sink(csdev);
 
 	return size;
-
 }
 static DEVICE_ATTR_RW(curr_sink);