From 86364c7b5e7233b6658d14ca2d49681e30af8ffe Mon Sep 17 00:00:00 2001
From: Phi-Bang Nguyen <pnguyen@baylibre.com>
Date: Fri, 2 Apr 2021 17:00:49 +0200
Subject: libcamera: pipeline: simple: Setup pipeline using subdev internal
 routing

When traversing the media graph to discover a pipeline from the camera
sensor to a video node, all sink-to-source paths inside subdevs are
considered. This can lead to invalid paths being followed, when a subdev
has restrictions on its internal routing.

The V4L2 API supports exposing subdev internal routing to userspace.
Make use if this feature, when implemented by a subdev, to restrict the
internal paths to the currently active routes. If a subdev doesn't
implement the internal routing operations, all source pads are
considered, as done today.

This change is needed to properly support multiple sensors connected to
the MediaTek i350 and i500 SENINF. Support for modifying subdev internal
routing isn't required at this point and will be added when a platform
will require this feature.

Signed-off-by: Phi-Bang Nguyen <pnguyen@baylibre.com>
Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
---
 src/libcamera/pipeline/simple/simple.cpp | 64 ++++++++++++++++++++++++++++++--
 1 file changed, 60 insertions(+), 4 deletions(-)

diff --git a/src/libcamera/pipeline/simple/simple.cpp b/src/libcamera/pipeline/simple/simple.cpp
index 239f77ff..e8957b77 100644
--- a/src/libcamera/pipeline/simple/simple.cpp
+++ b/src/libcamera/pipeline/simple/simple.cpp
@@ -236,6 +236,8 @@ public:
 	std::queue<std::map<unsigned int, FrameBuffer *>> converterQueue_;
 
 private:
+	static std::vector<const MediaPad *> routedSourcePads(MediaPad *sink);
+
 	void converterInputDone(FrameBuffer *buffer);
 	void converterOutputDone(FrameBuffer *buffer);
 };
@@ -360,12 +362,29 @@ SimpleCameraData::SimpleCameraData(SimplePipelineHandler *pipe,
 			break;
 		}
 
-		/* The actual breadth-first search algorithm. */
 		visited.insert(entity);
-		for (MediaPad *pad : entity->pads()) {
-			if (!(pad->flags() & MEDIA_PAD_FL_SOURCE))
-				continue;
 
+		/*
+		 * Add direct downstream entities to the search queue. If the
+		 * current entity supports the subdev internal routing API,
+		 * restrict the search to downstream entities reachable through
+		 * active routes.
+		 */
+
+		std::vector<const MediaPad *> pads;
+
+		if (sinkPad)
+			pads = routedSourcePads(sinkPad);
+
+		if (pads.empty()) {
+			for (const MediaPad *pad : entity->pads()) {
+				if (!(pad->flags() & MEDIA_PAD_FL_SOURCE))
+					continue;
+				pads.push_back(pad);
+			}
+		}
+
+		for (const MediaPad *pad : pads) {
 			for (MediaLink *link : pad->links()) {
 				MediaEntity *next = link->sink()->entity();
 				if (visited.find(next) == visited.end()) {
@@ -728,6 +747,43 @@ void SimpleCameraData::converterOutputDone(FrameBuffer *buffer)
 		pipe->completeRequest(request);
 }
 
+/* Retrieve all source pads connected to a sink pad through active routes. */
+std::vector<const MediaPad *> SimpleCameraData::routedSourcePads(MediaPad *sink)
+{
+	MediaEntity *entity = sink->entity();
+	std::unique_ptr<V4L2Subdevice> subdev =
+		std::make_unique<V4L2Subdevice>(entity);
+
+	int ret = subdev->open();
+	if (ret < 0)
+		return {};
+
+	std::vector<struct v4l2_subdev_route> routes;
+	ret = subdev->getRouting(&routes);
+	if (ret < 0)
+		return {};
+
+	std::vector<const MediaPad *> pads;
+
+	for (const struct v4l2_subdev_route &route : routes) {
+		if (sink->index() != route.sink_pad ||
+		    !(route.flags & V4L2_SUBDEV_ROUTE_FL_ACTIVE))
+			continue;
+
+		const MediaPad *pad = entity->getPadByIndex(route.source_pad);
+		if (!pad) {
+			LOG(SimplePipeline, Warning)
+				<< "Entity " << entity->name()
+				<< " has invalid route source pad "
+				<< route.source_pad;
+		}
+
+		pads.push_back(pad);
+	}
+
+	return pads;
+}
+
 /* -----------------------------------------------------------------------------
  * Camera Configuration
  */
-- 
cgit v1.2.1