update cti cscfg to call activate - deactivatecsn-6.4-rc4-cfs-etm-cti-v1

Signed-off-by: Mike Leach <mike.leach@linaro.org>

1 files changed, 78 insertions, 1 deletions

diff --git a/drivers/hwtracing/coresight/coresight-cti-core.c b/drivers/hwtracing/coresight/coresight-cti-core.c
index fd66e182f3cd..ddb61b9b412f 100644
--- a/drivers/hwtracing/coresight/coresight-cti-core.c
+++ b/drivers/hwtracing/coresight/coresight-cti-core.c
@@ -85,6 +85,66 @@ void cti_write_all_hw_regs(struct cti_drvdata *drvdata)
 
 	CS_LOCK(drvdata->base);
 }
+/*
+ * configs will always clear regs - we do not allow mixing config with
+ * existing CTI values on a single device - clear user configurable
+ * registers.
+ */
+static void cti_clear_user_cscfg_regs(struct cti_drvdata *drvdata)
+{
+	struct cti_config *config = &drvdata->config;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	for (i = 0; i < config->nr_trig_max; i++) {
+		config->ctiinen[i] = 0;
+		config->ctiouten[i] = 0;
+	}
+	config->asicctl = 0;
+	config->ctigate = 0;
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+}
+
+static int cti_cscfg_enable(struct coresight_device *csdev)
+{
+	struct cti_drvdata *drvdata = csdev_to_cti_drvdata(csdev);
+	unsigned long flags;
+	int rc = 0;
+	bool do_enable = false;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	/* not yet enabled if enable req_count = 0 */
+	if (!drvdata->config.enable_req_count)
+		do_enable = true;
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	/* activation of config and reg clear take the spinlock */
+	if (do_enable) {
+		cti_clear_user_cscfg_regs(drvdata);
+		rc = cscfg_csdev_enable_active_config(csdev);
+	}
+	return rc;
+}
+
+static void cti_cscfg_disable(struct coresight_device *csdev)
+{
+	struct cti_drvdata *drvdata = csdev_to_cti_drvdata(csdev);
+	unsigned long flags;
+	bool do_disable = false;
+
+	spin_lock_irqsave(&drvdata->spinlock, flags);
+	/* about to disable or failed enable if enable req_count <= 1 */
+	if (drvdata->config.enable_req_count <= 1)
+		do_disable = true;
+	spin_unlock_irqrestore(&drvdata->spinlock, flags);
+
+	/* de-activation of config and reg clear take the spinlock */
+	if (do_disable) {
+		cti_clear_user_cscfg_regs(drvdata);
+		cscfg_csdev_disable_active_config(csdev);
+	}
+}
 
 /* write regs to hardware and enable */
 static int cti_enable_hw(struct cti_drvdata *drvdata)
@@ -802,14 +862,31 @@ static void cti_pm_release(struct cti_drvdata *drvdata)
 int cti_enable(struct coresight_device *csdev, enum cs_mode mode, void *data)
 {
 	struct cti_drvdata *drvdata = csdev_to_cti_drvdata(csdev);
+	int rc = 0;
+
+	if (csdev->active_cscfg_hash)
+		rc = cti_cscfg_enable(csdev);
+
+	if (!rc) {
+		rc = cti_enable_hw(drvdata);
 
-	return cti_enable_hw(drvdata);
+		/* 
+		 * if the enable failed then rewind any 
+		 * config enable attempt
+		 */
+		if (rc && csdev->active_cscfg_hash)
+			cti_cscfg_disable(csdev);
+	}	       
+	return rc;
 }
 
 int cti_disable(struct coresight_device *csdev, void *data)
 {
 	struct cti_drvdata *drvdata = csdev_to_cti_drvdata(csdev);
 
+	if (csdev->active_cscfg_hash)
+		cti_cscfg_disable(csdev);
+
 	return cti_disable_hw(drvdata);
 }