1 files changed, 134 insertions, 144 deletions

diff --git a/drivers/staging/media/imx/imx-media-of.c b/drivers/staging/media/imx/imx-media-of.c
index 12df09f52490..acde372c6795 100644
--- a/drivers/staging/media/imx/imx-media-of.c
+++ b/drivers/staging/media/imx/imx-media-of.c
@@ -20,34 +20,6 @@
 #include <video/imx-ipu-v3.h>
 #include "imx-media.h"
 
-static int of_add_pad_link(struct imx_media_dev *imxmd,
-			   struct imx_media_pad *pad,
-			   struct device_node *local_sd_node,
-			   struct device_node *remote_sd_node,
-			   int local_pad, int remote_pad)
-{
-	dev_dbg(imxmd->md.dev, "%s: adding %s:%d -> %s:%d\n", __func__,
-		local_sd_node->name, local_pad,
-		remote_sd_node->name, remote_pad);
-
-	return imx_media_add_pad_link(imxmd, pad, remote_sd_node, NULL,
-				      local_pad, remote_pad);
-}
-
-static void of_parse_sensor(struct imx_media_dev *imxmd,
-			    struct imx_media_subdev *sensor,
-			    struct device_node *sensor_np)
-{
-	struct device_node *endpoint;
-
-	endpoint = of_graph_get_next_endpoint(sensor_np, NULL);
-	if (endpoint) {
-		v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint),
-					   &sensor->sensor_ep);
-		of_node_put(endpoint);
-	}
-}
-
 static int of_get_port_count(const struct device_node *np)
 {
 	struct device_node *ports, *child;
@@ -67,15 +39,14 @@ static int of_get_port_count(const struct device_node *np)
 }
 
 /*
- * find the remote device node and remote port id (remote pad #)
- * given local endpoint node
+ * find the remote device node given local endpoint node
  */
-static void of_get_remote_pad(struct device_node *epnode,
-			      struct device_node **remote_node,
-			      int *remote_pad)
+static bool of_get_remote(struct device_node *epnode,
+			  struct device_node **remote_node)
 {
 	struct device_node *rp, *rpp;
 	struct device_node *remote;
+	bool is_csi_port;
 
 	rp = of_graph_get_remote_port(epnode);
 	rpp = of_graph_get_remote_port_parent(epnode);
@@ -83,13 +54,12 @@ static void of_get_remote_pad(struct device_node *epnode,
 	if (of_device_is_compatible(rpp, "fsl,imx6q-ipu")) {
 		/* the remote is one of the CSI ports */
 		remote = rp;
-		*remote_pad = 0;
 		of_node_put(rpp);
+		is_csi_port = true;
 	} else {
 		remote = rpp;
-		if (of_property_read_u32(rp, "reg", remote_pad))
-			*remote_pad = 0;
 		of_node_put(rp);
+		is_csi_port = false;
 	}
 
 	if (!of_device_is_available(remote)) {
@@ -98,130 +68,66 @@ static void of_get_remote_pad(struct device_node *epnode,
 	} else {
 		*remote_node = remote;
 	}
+
+	return is_csi_port;
 }
 
 static int
 of_parse_subdev(struct imx_media_dev *imxmd, struct device_node *sd_np,
-		bool is_csi_port, struct imx_media_subdev **subdev)
+		bool is_csi_port)
 {
-	struct imx_media_subdev *imxsd;
-	int i, num_pads, ret;
+	int i, num_ports, ret;
 
 	if (!of_device_is_available(sd_np)) {
 		dev_dbg(imxmd->md.dev, "%s: %s not enabled\n", __func__,
 			sd_np->name);
-		*subdev = NULL;
 		/* unavailable is not an error */
 		return 0;
 	}
 
 	/* register this subdev with async notifier */
-	imxsd = imx_media_add_async_subdev(imxmd, sd_np, NULL);
-	ret = PTR_ERR_OR_ZERO(imxsd);
+	ret = imx_media_add_async_subdev(imxmd, of_fwnode_handle(sd_np),
+					 NULL);
 	if (ret) {
 		if (ret == -EEXIST) {
 			/* already added, everything is fine */
-			*subdev = NULL;
 			return 0;
 		}
 
 		/* other error, can't continue */
 		return ret;
 	}
-	*subdev = imxsd;
-
-	if (is_csi_port) {
-		/*
-		 * the ipu-csi has one sink port and two source ports.
-		 * The source ports are not represented in the device tree,
-		 * but are described by the internal pads and links later.
-		 */
-		num_pads = CSI_NUM_PADS;
-		imxsd->num_sink_pads = CSI_NUM_SINK_PADS;
-	} else if (of_device_is_compatible(sd_np, "fsl,imx6-mipi-csi2")) {
-		num_pads = of_get_port_count(sd_np);
-		/* the mipi csi2 receiver has only one sink port */
-		imxsd->num_sink_pads = 1;
-	} else if (of_device_is_compatible(sd_np, "video-mux")) {
-		num_pads = of_get_port_count(sd_np);
-		/* for the video mux, all but the last port are sinks */
-		imxsd->num_sink_pads = num_pads - 1;
-	} else {
-		num_pads = of_get_port_count(sd_np);
-		if (num_pads != 1) {
-			/* confused, but no reason to give up here */
-			dev_warn(imxmd->md.dev,
-				 "%s: unknown device %s with %d ports\n",
-				 __func__, sd_np->name, num_pads);
-			return 0;
-		}
-
-		/*
-		 * we got to this node from this single source port,
-		 * there are no sink pads.
-		 */
-		imxsd->num_sink_pads = 0;
-	}
-
-	if (imxsd->num_sink_pads >= num_pads)
-		return -EINVAL;
-
-	imxsd->num_src_pads = num_pads - imxsd->num_sink_pads;
-
-	dev_dbg(imxmd->md.dev, "%s: %s has %d pads (%d sink, %d src)\n",
-		__func__, sd_np->name, num_pads,
-		imxsd->num_sink_pads, imxsd->num_src_pads);
 
 	/*
-	 * With no sink, this subdev node is the original source
-	 * of video, parse it's media bus for use by the pipeline.
+	 * the ipu-csi has one sink port. The source pads are not
+	 * represented in the device tree by port nodes, but are
+	 * described by the internal pads and links later.
 	 */
-	if (imxsd->num_sink_pads == 0)
-		of_parse_sensor(imxmd, imxsd, sd_np);
+	num_ports = is_csi_port ? 1 : of_get_port_count(sd_np);
 
-	for (i = 0; i < num_pads; i++) {
+	for (i = 0; i < num_ports; i++) {
 		struct device_node *epnode = NULL, *port, *remote_np;
-		struct imx_media_subdev *remote_imxsd;
-		struct imx_media_pad *pad;
-		int remote_pad;
-
-		/* init this pad */
-		pad = &imxsd->pad[i];
-		pad->pad.flags = (i < imxsd->num_sink_pads) ?
-			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
-
-		if (is_csi_port)
-			port = (i < imxsd->num_sink_pads) ? sd_np : NULL;
-		else
-			port = of_graph_get_port_by_id(sd_np, i);
+
+		port = is_csi_port ? sd_np : of_graph_get_port_by_id(sd_np, i);
 		if (!port)
 			continue;
 
 		for_each_child_of_node(port, epnode) {
-			of_get_remote_pad(epnode, &remote_np, &remote_pad);
+			bool remote_is_csi;
+
+			remote_is_csi = of_get_remote(epnode, &remote_np);
 			if (!remote_np)
 				continue;
 
-			ret = of_add_pad_link(imxmd, pad, sd_np, remote_np,
-					      i, remote_pad);
+			ret = of_parse_subdev(imxmd, remote_np, remote_is_csi);
+			of_node_put(remote_np);
 			if (ret)
 				break;
-
-			if (i < imxsd->num_sink_pads) {
-				/* follow sink endpoints upstream */
-				ret = of_parse_subdev(imxmd, remote_np,
-						      false, &remote_imxsd);
-				if (ret)
-					break;
-			}
-
-			of_node_put(remote_np);
 		}
 
 		if (port != sd_np)
 			of_node_put(port);
 		if (ret) {
-			of_node_put(remote_np);
 			of_node_put(epnode);
 			break;
 		}
@@ -230,13 +136,10 @@ of_parse_subdev(struct imx_media_dev *imxmd, struct device_node *sd_np,
 	return ret;
 }
 
-int imx_media_of_parse(struct imx_media_dev *imxmd,
-		       struct imx_media_subdev *(*csi)[4],
-		       struct device_node *np)
+int imx_media_add_of_subdevs(struct imx_media_dev *imxmd,
+			     struct device_node *np)
 {
-	struct imx_media_subdev *lcsi;
 	struct device_node *csi_np;
-	u32 ipu_id, csi_id;
 	int i, ret;
 
 	for (i = 0; ; i++) {
@@ -244,33 +147,120 @@ int imx_media_of_parse(struct imx_media_dev *imxmd,
 		if (!csi_np)
 			break;
 
-		ret = of_parse_subdev(imxmd, csi_np, true, &lcsi);
+		ret = of_parse_subdev(imxmd, csi_np, true);
+		of_node_put(csi_np);
 		if (ret)
-			goto err_put;
+			return ret;
+	}
 
-		ret = of_property_read_u32(csi_np, "reg", &csi_id);
-		if (ret) {
-			dev_err(imxmd->md.dev,
-				"%s: csi port missing reg property!\n",
-				__func__);
-			goto err_put;
-		}
+	return 0;
+}
 
-		ipu_id = of_alias_get_id(csi_np->parent, "ipu");
-		of_node_put(csi_np);
+/*
+ * Create a single media link to/from sd using a fwnode link.
+ *
+ * NOTE: this function assumes an OF port node is equivalent to
+ * a media pad (port id equal to media pad index), and that an
+ * OF endpoint node is equivalent to a media link.
+ */
+static int create_of_link(struct imx_media_dev *imxmd,
+			  struct v4l2_subdev *sd,
+			  struct v4l2_fwnode_link *link)
+{
+	struct v4l2_subdev *remote, *src, *sink;
+	int src_pad, sink_pad;
 
-		if (ipu_id > 1 || csi_id > 1) {
-			dev_err(imxmd->md.dev,
-				"%s: invalid ipu/csi id (%u/%u)\n",
-				__func__, ipu_id, csi_id);
-			return -EINVAL;
-		}
+	if (link->local_port >= sd->entity.num_pads)
+		return -EINVAL;
+
+	remote = imx_media_find_subdev_by_fwnode(imxmd, link->remote_node);
+	if (!remote)
+		return 0;
+
+	if (sd->entity.pads[link->local_port].flags & MEDIA_PAD_FL_SINK) {
+		src = remote;
+		src_pad = link->remote_port;
+		sink = sd;
+		sink_pad = link->local_port;
+	} else {
+		src = sd;
+		src_pad = link->local_port;
+		sink = remote;
+		sink_pad = link->remote_port;
+	}
+
+	/* make sure link doesn't already exist before creating */
+	if (media_entity_find_link(&src->entity.pads[src_pad],
+				   &sink->entity.pads[sink_pad]))
+		return 0;
+
+	v4l2_info(sd->v4l2_dev, "%s:%d -> %s:%d\n",
+		  src->name, src_pad, sink->name, sink_pad);
+
+	return media_create_pad_link(&src->entity, src_pad,
+				     &sink->entity, sink_pad, 0);
+}
+
+/*
+ * Create media links to/from sd using its device-tree endpoints.
+ */
+int imx_media_create_of_links(struct imx_media_dev *imxmd,
+			      struct v4l2_subdev *sd)
+{
+	struct v4l2_fwnode_link link;
+	struct device_node *ep;
+	int ret;
+
+	for_each_endpoint_of_node(sd->dev->of_node, ep) {
+		ret = v4l2_fwnode_parse_link(of_fwnode_handle(ep), &link);
+		if (ret)
+			continue;
 
-		(*csi)[ipu_id * 2 + csi_id] = lcsi;
+		ret = create_of_link(imxmd, sd, &link);
+		v4l2_fwnode_put_link(&link);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+/*
+ * Create media links to the given CSI subdevice's sink pads,
+ * using its device-tree endpoints.
+ */
+int imx_media_create_csi_of_links(struct imx_media_dev *imxmd,
+				  struct v4l2_subdev *csi)
+{
+	struct device_node *csi_np = csi->dev->of_node;
+	struct fwnode_handle *fwnode, *csi_ep;
+	struct v4l2_fwnode_link link;
+	struct device_node *ep;
+	int ret;
+
+	link.local_node = of_fwnode_handle(csi_np);
+	link.local_port = CSI_SINK_PAD;
+
+	for_each_child_of_node(csi_np, ep) {
+		csi_ep = of_fwnode_handle(ep);
+
+		fwnode = fwnode_graph_get_remote_endpoint(csi_ep);
+		if (!fwnode)
+			continue;
+
+		fwnode = fwnode_get_parent(fwnode);
+		fwnode_property_read_u32(fwnode, "reg", &link.remote_port);
+		fwnode = fwnode_get_next_parent(fwnode);
+		if (is_of_node(fwnode) &&
+		    of_node_cmp(to_of_node(fwnode)->name, "ports") == 0)
+			fwnode = fwnode_get_next_parent(fwnode);
+		link.remote_node = fwnode;
+
+		ret = create_of_link(imxmd, csi, &link);
+		fwnode_handle_put(link.remote_node);
+		if (ret)
+			return ret;
 	}
 
 	return 0;
-err_put:
-	of_node_put(csi_np);
-	return ret;
 }