[PATCH 4/4] coresight-tpda: Add support multi-port input on TPDA

[Tao Zhang <quic_taozha@xxxxxxxxxxx>]

Since the funnel supports multi-port output scenarios, there may
be more than one TPDM connected to one TPDA input port.  In this
way, when reading the element size of the TPDM, TPDA driver needs
to find the correct TPDM corresponding to the input port. When
TPDA finds a TPDM on an input port, it read the device tree of
the TPDM and finds the configured TPDA input port number. If it
is the same as the input port number passed into the function,
then it is the correct TPDM that needs to be found.

Signed-off-by: Tao Zhang <quic_taozha@xxxxxxxxxxx>
---
 drivers/hwtracing/coresight/coresight-tpda.c | 27 +++++++++++++++++++-
 1 file changed, 26 insertions(+), 1 deletion(-)

diff --git a/drivers/hwtracing/coresight/coresight-tpda.c b/drivers/hwtracing/coresight/coresight-tpda.c
index 52b0201090fb..ba71e1ff18e3 100644
--- a/drivers/hwtracing/coresight/coresight-tpda.c
+++ b/drivers/hwtracing/coresight/coresight-tpda.c
@@ -84,6 +84,26 @@ static int tpdm_read_element_size(struct tpda_drvdata *drvdata,
 	return rc;
 }
 
+/*
+ * Check if the parameter of the input port number in "tpda_get_element_size"
+ * is the same as the property of the TPDA input port number defined in the device
+ * tree.
+ * Return true if they are the same or the property is not read.
+ * Otherwise, return false.
+ */
+static bool is_tpda_inport_matched(struct coresight_device *csdev, u32 tpda_inport)
+{
+	int rc = -EINVAL;
+	u32 inport_nr;
+
+	rc = fwnode_property_read_u32(dev_fwnode(csdev->dev.parent),
+			"qcom,tpda-input-port", &inport_nr);
+	if (!rc)
+		return (inport_nr == tpda_inport);
+
+	return true;
+}
+
 /*
  * Search and read element data size from the TPDM node in
  * the devicetree. Each input port of TPDA is connected to
@@ -99,6 +119,10 @@ static int tpda_get_element_size(struct tpda_drvdata *drvdata,
 	int rc = 0;
 	int i;
 	struct coresight_device *in;
+	static u32 tpda_inport;
+
+	if (inport != -1)
+		tpda_inport = inport;
 
 	for (i = 0; i < csdev->pdata->nr_inconns; i++) {
 		in = csdev->pdata->in_conns[i]->src_dev;
@@ -110,7 +134,8 @@ static int tpda_get_element_size(struct tpda_drvdata *drvdata,
 		    csdev->pdata->in_conns[i]->dest_port != inport)
 			continue;
 
-		if (coresight_device_is_tpdm(in)) {
+		if (coresight_device_is_tpdm(in) &&
+				(is_tpda_inport_matched(in, tpda_inport))) {
 			if (drvdata->dsb_esize || drvdata->cmb_esize)
 				return -EEXIST;
 			rc = tpdm_read_element_size(drvdata, in);
-- 
2.17.1