[PATCH v2 4/9] coresight-tpdm: Add DSB dataset support

[Mao Jinlong <quic_jinlmao@xxxxxxxxxxx>]

TPDM serves as data collection component for various dataset types.
DSB(Discrete Single Bit) is one of the dataset types. DSB subunit
can be enabled for data collection by writing 1 to the first bit of
DSB_CR register. This change is to add enable/disable function for
DSB dataset by writing DSB_CR register.

Signed-off-by: Tao Zhang <quic_taozha@xxxxxxxxxxx>
Signed-off-by: Mao Jinlong <quic_jinlmao@xxxxxxxxxxx>
---
 drivers/hwtracing/coresight/coresight-tpdm.c | 56 ++++++++++++++++++++
 drivers/hwtracing/coresight/coresight-tpdm.h | 23 ++++++++
 2 files changed, 79 insertions(+)

diff --git a/drivers/hwtracing/coresight/coresight-tpdm.c b/drivers/hwtracing/coresight/coresight-tpdm.c
index f494cef4fb24..c0dd216f70eb 100644
--- a/drivers/hwtracing/coresight/coresight-tpdm.c
+++ b/drivers/hwtracing/coresight/coresight-tpdm.c
@@ -20,6 +20,27 @@
 
 DEFINE_CORESIGHT_DEVLIST(tpdm_devs, "tpdm");
 
+static void tpdm_enable_dsb(struct tpdm_drvdata *drvdata)
+{
+	u32 val;
+
+	/* Set the enable bit of DSB control register to 1 */
+	val = readl_relaxed(drvdata->base + TPDM_DSB_CR);
+	val = val | BIT(0);
+	writel_relaxed(val, drvdata->base + TPDM_DSB_CR);
+}
+
+static void _tpdm_enable(struct tpdm_drvdata *drvdata)
+{
+	CS_UNLOCK(drvdata->base);
+
+	/* Check if DSB datasets is present for TPDM. */
+	if (test_bit(TPDM_DS_DSB, drvdata->datasets))
+		tpdm_enable_dsb(drvdata);
+
+	CS_LOCK(drvdata->base);
+}
+
 static int tpdm_enable(struct coresight_device *csdev,
 		       struct perf_event *event, u32 mode)
 {
@@ -31,6 +52,7 @@ static int tpdm_enable(struct coresight_device *csdev,
 		return -EBUSY;
 	}
 
+	_tpdm_enable(drvdata);
 	drvdata->enable = true;
 	mutex_unlock(&drvdata->lock);
 
@@ -38,6 +60,28 @@ static int tpdm_enable(struct coresight_device *csdev,
 	return 0;
 }
 
+static void tpdm_disable_dsb(struct tpdm_drvdata *drvdata)
+{
+	u32 val;
+
+	/* Set the enable bit of DSB control register to 0 */
+	val = readl_relaxed(drvdata->base + TPDM_DSB_CR);
+	val = val & ~BIT(0);
+	writel_relaxed(val, drvdata->base + TPDM_DSB_CR);
+}
+
+static void _tpdm_disable(struct tpdm_drvdata *drvdata)
+{
+	CS_UNLOCK(drvdata->base);
+
+	/* Check if DSB datasets is present for TPDM. */
+	if (test_bit(TPDM_DS_DSB, drvdata->datasets))
+		tpdm_disable_dsb(drvdata);
+
+	CS_LOCK(drvdata->base);
+
+}
+
 static void tpdm_disable(struct coresight_device *csdev,
 			 struct perf_event *event)
 {
@@ -49,6 +93,7 @@ static void tpdm_disable(struct coresight_device *csdev,
 		return;
 	}
 
+	_tpdm_disable(drvdata);
 	drvdata->enable = false;
 	mutex_unlock(&drvdata->lock);
 
@@ -75,8 +120,19 @@ static const struct coresight_ops tpdm_cs_ops = {
 static void tpdm_init_default_data(struct tpdm_drvdata *drvdata)
 {
 	static int traceid = TPDM_TRACE_ID_START;
+	int i;
+	u32 pidr;
 
+	CS_UNLOCK(drvdata->base);
 	drvdata->traceid = traceid++;
+
+	/*  Get the datasets present on the TPDM. */
+	pidr = readl_relaxed(drvdata->base + CORESIGHT_PERIPHIDR0);
+	for (i = 0; i < TPDM_DATASETS; i++) {
+		if (pidr & BIT(i))
+			__set_bit(i, drvdata->datasets);
+	}
+	CS_LOCK(drvdata->base);
 }
 
 static int tpdm_probe(struct amba_device *adev, const struct amba_id *id)
diff --git a/drivers/hwtracing/coresight/coresight-tpdm.h b/drivers/hwtracing/coresight/coresight-tpdm.h
index 980ae90ff1c8..ba99c20e12bb 100644
--- a/drivers/hwtracing/coresight/coresight-tpdm.h
+++ b/drivers/hwtracing/coresight/coresight-tpdm.h
@@ -8,6 +8,27 @@
 
 /* Default value of the traceid */
 #define TPDM_TRACE_ID_START 128
+/* The max number of the datasets that TPDM supports */
+#define TPDM_DATASETS       7
+
+/* DSB Subunit Registers */
+#define TPDM_DSB_CR		(0x780)
+
+/**
+ * This enum is for PERIPHIDR0 register of TPDM.
+ * The fields [6:0] of PERIPHIDR0 are used to determine what
+ * interfaces and subunits are present on a given TPDM.
+ *
+ * PERIPHIDR0[0] : Fix to 1 if ImplDef subunit present, else 0.
+ *                 No TPDM supports ImplDef subunit now. But
+ *                 the first bit of PERIPHIDR0 is for ImplDef
+ *                 subunit for initial design.
+ * PERIPHIDR0[1] : Fix to 1 if DSB subunit present, else 0.
+ */
+enum tpdm_dataset {
+	TPDM_DS_IMPLDEF,
+	TPDM_DS_DSB,
+};
 
 /**
  * struct tpdm_drvdata - specifics associated to an TPDM component
@@ -16,6 +37,7 @@
  * @csdev:      component vitals needed by the framework.
  * @lock:       lock for the enable value.
  * @enable:     enable status of the component.
+ * @datasets:   The datasets types present of the TPDM.
  * @traceid:    value of the current ID for this component.
  */
 
@@ -25,6 +47,7 @@ struct tpdm_drvdata {
 	struct coresight_device	*csdev;
 	struct mutex		lock;
 	bool			enable;
+	DECLARE_BITMAP(datasets, TPDM_DATASETS);
 	int			traceid;
 };
 
-- 
2.17.1