[PATCH v1 7/8] coresight-tpda: Add support to configure CMB element size

[Mao Jinlong <quic_jinlmao@xxxxxxxxxxx>]

Read the CMB element size from the device tree. Set the register
bit that controls the CMB element size of the corresponding port.

Signed-off-by: Mao Jinlong <quic_jinlmao@xxxxxxxxxxx>
---
 drivers/hwtracing/coresight/coresight-tpda.c | 33 +++++++++++++++++---
 drivers/hwtracing/coresight/coresight-tpda.h |  4 +++
 2 files changed, 32 insertions(+), 5 deletions(-)

diff --git a/drivers/hwtracing/coresight/coresight-tpda.c b/drivers/hwtracing/coresight/coresight-tpda.c
index a620a51e861a..c2ce62e769ea 100644
--- a/drivers/hwtracing/coresight/coresight-tpda.c
+++ b/drivers/hwtracing/coresight/coresight-tpda.c
@@ -42,11 +42,12 @@ static int tpda_set_element_size(struct tpda_drvdata *drvdata,
 				continue;
 		}
 		if (in_csdev && strstr(dev_name(&in_csdev->dev), "tpdm")) {
-			if (!of_property_read_u32(in_csdev->dev.parent->of_node,
-					"qcom,dsb-elemenet-size", &drvdata->dsb_esize[nr_inport]))
-				break;
-			dev_err(drvdata->dev, "Fail to get data set element size\n");
-			return -EINVAL;
+			of_property_read_u32(in_csdev->dev.parent->of_node,
+					"qcom,dsb-elemenet-size", &drvdata->dsb_esize[nr_inport]);
+			of_property_read_u8(in_csdev->dev.parent->of_node,
+					"qcom,cmb-elemenet-size", &drvdata->cmb_esize[nr_inport]);
+
+			break;
 		}
 		tpda_set_element_size(drvdata, in_csdev, 0);
 	}
@@ -82,6 +83,28 @@ static void tpda_enable_port(struct tpda_drvdata *drvdata, int port)
 	else
 		dev_err(drvdata->dev,
 			"DSB data size input from port[%d] is invalid\n", port);
+
+	/*
+	 * Configure aggregator port n CMB data set element size
+	 * Set the bit to 0 if the size is 8
+	 * Set the bit to 1 if the size is 32
+	 * Set the bit to 1 if the size is 64
+	 */
+	if (drvdata->cmb_esize[port] == 8)
+		val &= ~TPDA_Pn_CR_CMBSIZE;
+	else if (drvdata->cmb_esize[port] == 32)
+		val |= FIELD_PREP(TPDA_Pn_CR_CMBSIZE, 0x1);
+	else if (drvdata->cmb_esize[port] == 32)
+		val |= FIELD_PREP(TPDA_Pn_CR_CMBSIZE, 0x2);
+	else {
+		/*
+		 * CMB element size is not configured.
+		 * Fall back to 32-bit.
+		 */
+		WARN_ON_ONCE(1);
+		val |= FIELD_PREP(TPDA_Pn_CR_CMBSIZE, 0x1);
+	}
+
 	/* Enable the port */
 	val |= TPDA_Pn_CR_ENA;
 	writel_relaxed(val, drvdata->base + TPDA_Pn_CR(port));
diff --git a/drivers/hwtracing/coresight/coresight-tpda.h b/drivers/hwtracing/coresight/coresight-tpda.h
index 9ec5870b5f7c..d5290d21457d 100644
--- a/drivers/hwtracing/coresight/coresight-tpda.h
+++ b/drivers/hwtracing/coresight/coresight-tpda.h
@@ -12,6 +12,8 @@
 #define TPDA_Pn_CR_ENA		BIT(0)
 /* Aggregator port DSB data set element size bit */
 #define TPDA_Pn_CR_DSBSIZE		BIT(8)
+/* Aggregator port CMB data set element size bit */
+#define TPDA_Pn_CR_CMBSIZE	GENMASK(7, 6)
 
 #define TPDA_MAX_INPORTS	32
 
@@ -26,6 +28,7 @@
  * @spinlock:   lock for the drvdata value.
  * @enable:     enable status of the component.
  * @dsb_esize:   DSB element size
+ * @cmb_esize:	CMB element size. Must be 8, 32 or 64.
  */
 struct tpda_drvdata {
 	void __iomem		*base;
@@ -34,6 +37,7 @@ struct tpda_drvdata {
 	spinlock_t		spinlock;
 	u8			atid;
 	u32			dsb_esize[TPDA_MAX_INPORTS];
+	u8			cmb_esize[TPDA_MAX_INPORTS];
 };
 
 #endif  /* _CORESIGHT_CORESIGHT_TPDA_H */
-- 
2.39.0