[PATCH v2 5/9] coresight: Dynamically add connections

[James Clark]

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 at arm.com>
---
 .../hwtracing/coresight/coresight-platform.c  | 77 ++++++++++++++-----
 include/linux/coresight.h                     |  7 +-
 2 files changed, 64 insertions(+), 20 deletions(-)

diff --git a/drivers/hwtracing/coresight/coresight-platform.c b/drivers/hwtracing/coresight/coresight-platform.c
index c77238cdf448..16553f7dde12 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->remote_fwnode && conn->port != -1 &&
+		    i->port == conn->port) {
+			dev_warn(dev, "Duplicate output port %d\n", i->port);
+			return -EINVAL;
+		}
+		if (!i->remote_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->remote_fwnode) {
-			dev_warn(dev, "Duplicate output port %d\n",
-				 endpoint.port);
-			ret = -EINVAL;
-			break;
-		}
-		conn->port = endpoint.port;
+		conn.port = 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->remote_fwnode = fwnode_handle_get(rdev_fwnode);
-		conn->remote_port = rendpoint.port;
+		conn.remote_fwnode = fwnode_handle_get(rdev_fwnode);
+		conn.remote_port = rendpoint.port;
+
+		ret = coresight_add_conn(dev, pdata, &conn);
+		if (ret) {
+			fwnode_handle_put(conn.remote_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].port;
-
-		/* Duplicate output port */
-		WARN_ON(pdata->out_conns[port].remote_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 64bb5fc95afa..47fa58d6981d 100644
--- a/include/linux/coresight.h
+++ b/include/linux/coresight.h
@@ -173,7 +173,9 @@ struct coresight_desc {
  *		 on the remote device.
  * @remote_fwnode: remote component's fwnode handle.
  * @remote_dev:  a @coresight_device representation of the component
- *		 connected to @port.
+ *               connected to @port. Will be looked up using
+ *               @remote_fwnode once the remote's coresight_device has
+ *               been created.
  * @link: Representation of the connection as a sysfs link.
  */
 struct coresight_connection {
@@ -614,5 +616,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