[PATCH 4/8] coresight: Dynamically add connections

[James Clark <james.clark@arm.com>]

Add a function for adding connections dynamically. This also removes
the 1:1 mapping between port number and the index into the connections
array. The only place this mapping was used was in the warning for
duplicate output ports, which has been replaced by a search. Other
uses of the port number already use the port member variable.

Being able to dynamically add connections will allow other devices like
CTI to re-use the connection mechanism despite not having explicit
connections described in the DT.

Signed-off-by: James Clark <james.clark@arm.com>
---
 .../hwtracing/coresight/coresight-platform.c  | 77 ++++++++++++++-----
 include/linux/coresight.h                     |  7 +-
 2 files changed, 63 insertions(+), 21 deletions(-)

diff --git a/drivers/hwtracing/coresight/coresight-platform.c b/drivers/hwtracing/coresight/coresight-platform.c
index ed865e0621a9..8effce5937d9 100644
--- a/drivers/hwtracing/coresight/coresight-platform.c
+++ b/drivers/hwtracing/coresight/coresight-platform.c
@@ -27,8 +27,9 @@ static int coresight_alloc_conns(struct device *dev,
 				 struct coresight_platform_data *pdata)
 {
 	if (pdata->nr_outconns) {
-		pdata->out_conns = devm_kcalloc(dev, pdata->nr_outconns,
-					    sizeof(*pdata->out_conns), GFP_KERNEL);
+		pdata->out_conns = devm_krealloc_array(
+			dev, pdata->out_conns, pdata->nr_outconns,
+			sizeof(*pdata->out_conns), GFP_KERNEL | __GFP_ZERO);
 		if (!pdata->out_conns)
 			return -ENOMEM;
 	}
@@ -36,6 +37,48 @@ static int coresight_alloc_conns(struct device *dev,
 	return 0;
 }
 
+/*
+ * Add a connection in the first free slot, or realloc
+ * if there is no space. @conn's contents is copied into the new slot.
+ *
+ * If the output port is already assigned on this device, return -EINVAL
+ */
+int coresight_add_conn(struct device *dev,
+		       struct coresight_platform_data *pdata,
+		       const struct coresight_connection *conn)
+{
+	int ret;
+	struct coresight_connection *free_conn = NULL;
+	struct coresight_connection *i;
+
+	/*
+	 * Search for a free slot, and while looking for one, warn
+	 * on any existing duplicate output port.
+	 */
+	for (i = pdata->out_conns; i < pdata->out_conns + pdata->nr_outconns;
+	     ++i) {
+		if (i->child_fwnode && conn->outport != -1 &&
+		    i->outport == conn->outport) {
+			dev_warn(dev, "Duplicate output port %d\n", i->outport);
+			return -EINVAL;
+		}
+		if (!i->child_fwnode && !free_conn)
+			free_conn = i;
+	}
+
+	if (!free_conn) {
+		pdata->nr_outconns++;
+		ret = coresight_alloc_conns(dev, pdata);
+		if (ret)
+			return ret;
+		free_conn = &pdata->out_conns[pdata->nr_outconns - 1];
+	}
+
+	*free_conn = *conn;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(coresight_add_conn);
+
 static struct device *
 coresight_find_device_by_fwnode(struct fwnode_handle *fwnode)
 {
@@ -224,7 +267,7 @@ static int of_coresight_parse_endpoint(struct device *dev,
 	struct device_node *rep = NULL;
 	struct device *rdev = NULL;
 	struct fwnode_handle *rdev_fwnode;
-	struct coresight_connection *conn;
+	struct coresight_connection conn;
 
 	do {
 		/* Parse the local port details */
@@ -251,14 +294,7 @@ static int of_coresight_parse_endpoint(struct device *dev,
 			break;
 		}
 
-		conn = &pdata->out_conns[endpoint.port];
-		if (conn->child_fwnode) {
-			dev_warn(dev, "Duplicate output port %d\n",
-				 endpoint.port);
-			ret = -EINVAL;
-			break;
-		}
-		conn->outport = endpoint.port;
+		conn.outport = endpoint.port;
 		/*
 		 * Hold the refcount to the target device. This could be
 		 * released via:
@@ -267,8 +303,14 @@ static int of_coresight_parse_endpoint(struct device *dev,
 		 * 2) While removing the target device via
 		 *    coresight_remove_match()
 		 */
-		conn->child_fwnode = fwnode_handle_get(rdev_fwnode);
-		conn->child_port = rendpoint.port;
+		conn.child_fwnode = fwnode_handle_get(rdev_fwnode);
+		conn.child_port = rendpoint.port;
+
+		ret = coresight_add_conn(dev, pdata, &conn);
+		if (ret) {
+			fwnode_handle_put(conn.child_fwnode);
+			return ret;
+		}
 		/* Connection record updated */
 	} while (0);
 
@@ -741,13 +783,10 @@ static int acpi_coresight_parse_graph(struct acpi_device *adev,
 
 	/* Copy the connection information to the final location */
 	for (i = 0; conns + i < ptr; i++) {
-		int port = conns[i].outport;
-
-		/* Duplicate output port */
-		WARN_ON(pdata->out_conns[port].child_fwnode);
-		pdata->out_conns[port] = conns[i];
+		rc = coresight_add_conn(&adev->dev, pdata, &conns[i]);
+		if (rc)
+			return rc;
 	}
-
 	devm_kfree(&adev->dev, conns);
 	return 0;
 }
diff --git a/include/linux/coresight.h b/include/linux/coresight.h
index cdf0d1def778..62cc8e43b6cb 100644
--- a/include/linux/coresight.h
+++ b/include/linux/coresight.h
@@ -167,8 +167,8 @@ struct coresight_desc {
  * @outport:	a connection's output port number.
  * @child_port:	remote component's port number @output is connected to.
  * @chid_fwnode: remote component's fwnode handle.
- * @child_dev:	a @coresight_device representation of the component
-		connected to @outport.
+ * @child_dev:  remote component's @coresight_device representation. Will
+ *		be looked up and assigned using @child_fwnode
  * @link: Representation of the connection as a sysfs link.
  */
 struct coresight_connection {
@@ -609,5 +609,8 @@ static inline void coresight_write64(struct coresight_device *csdev, u64 val, u3
 extern int coresight_get_cpu(struct device *dev);
 
 struct coresight_platform_data *coresight_get_platform_data(struct device *dev);
+int coresight_add_conn(struct device *dev,
+		       struct coresight_platform_data *pdata,
+		       const struct coresight_connection *conn);
 
 #endif		/* _LINUX_COREISGHT_H */
-- 
2.34.1