RASPBERRYPI ONLY: libcamera: v4l2_videodevice: Limit number of queued buffers

V4L2 only allows upto VIDEO_MAX_FRAME frames to be queued at a time, so
if we reach this limit, store the framebuffers in a pending queue, and
try to enqueue once a buffer has been dequeued.

Signed-off-by: Naushir Patuck <naush@raspberrypi.com>

2 files changed, 203 insertions, 136 deletions

diff --git a/include/libcamera/internal/v4l2_videodevice.h b/include/libcamera/internal/v4l2_videodevice.h
index 9057be08..d077f958 100644
--- a/include/libcamera/internal/v4l2_videodevice.h
+++ b/include/libcamera/internal/v4l2_videodevice.h
@@ -12,6 +12,7 @@
 #include <memory>
 #include <optional>
 #include <ostream>
+#include <queue>
 #include <stdint.h>
 #include <string>
 #include <unordered_set>
@@ -266,6 +267,8 @@ private:
 	void bufferAvailable();
 	FrameBuffer *dequeueBuffer();
 
+	int queueToDevice(FrameBuffer *buffer);
+
 	void watchdogExpired();
 
 	template<typename T>
@@ -281,6 +284,7 @@ private:
 
 	V4L2BufferCache *cache_;
 	std::map<unsigned int, FrameBuffer *> queuedBuffers_;
+	std::queue<FrameBuffer *> pendingBuffersToQueue_;
 
 	EventNotifier *fdBufferNotifier_;
 
diff --git a/src/libcamera/v4l2_videodevice.cpp b/src/libcamera/v4l2_videodevice.cpp
index 4947aa3d..03ce5df0 100644
--- a/src/libcamera/v4l2_videodevice.cpp
+++ b/src/libcamera/v4l2_videodevice.cpp
@@ -1545,7 +1545,7 @@ int V4L2VideoDevice::releaseBuffers()
 }
 
 /**
- * \brief Queue a buffer to the video device
+ * \brief Queue a buffer to the video device if possible
  * \param[in] buffer The buffer to be queued
  *
  * For capture video devices the \a buffer will be filled with data by the
@@ -1559,156 +1559,38 @@ int V4L2VideoDevice::releaseBuffers()
  * Note that queueBuffer() will fail if the device is in the process of being
  * stopped from a streaming state through streamOff().
  *
+ * V4L2 only allows upto VIDEO_MAX_FRAME frames to be queued at a time, so if
+ * we reach this limit, store the framebuffers in a pending queue, and try to
+ * enqueue once a buffer has been dequeued.
+ *
  * \return 0 on success or a negative error code otherwise
  */
 int V4L2VideoDevice::queueBuffer(FrameBuffer *buffer)
 {
-	struct v4l2_plane v4l2Planes[VIDEO_MAX_PLANES] = {};
-	struct v4l2_buffer buf = {};
-	int ret;
-
 	if (state_ == State::Stopping) {
 		LOG(V4L2, Error) << "Device is in a stopping state.";
 		return -ESHUTDOWN;
 	}
 
-	/*
-	 * Pipeline handlers should not requeue buffers after releasing the
-	 * buffers on the device. Any occurence of this error should be fixed
-	 * in the pipeline handler directly.
-	 */
-	if (!cache_) {
-		LOG(V4L2, Fatal) << "No BufferCache available to queue.";
-		return -ENOENT;
-	}
-
-	ret = cache_->get(*buffer);
-	if (ret < 0)
-		return ret;
-
-	buf.index = ret;
-	buf.type = bufferType_;
-	buf.memory = memoryType_;
-	buf.field = V4L2_FIELD_NONE;
-
-	bool multiPlanar = V4L2_TYPE_IS_MULTIPLANAR(buf.type);
-	const std::vector<FrameBuffer::Plane> &planes = buffer->planes();
-	const unsigned int numV4l2Planes = format_.planesCount;
-
-	/*
-	 * Ensure that the frame buffer has enough planes, and that they're
-	 * contiguous if the V4L2 format requires them to be.
-	 */
-	if (planes.size() < numV4l2Planes) {
-		LOG(V4L2, Error) << "Frame buffer has too few planes";
-		return -EINVAL;
-	}
-
-	if (planes.size() != numV4l2Planes && !buffer->_d()->isContiguous()) {
-		LOG(V4L2, Error) << "Device format requires contiguous buffer";
-		return -EINVAL;
-	}
-
-	if (buf.memory == V4L2_MEMORY_DMABUF) {
-		if (multiPlanar) {
-			for (unsigned int p = 0; p < numV4l2Planes; ++p)
-				v4l2Planes[p].m.fd = planes[p].fd.get();
-		} else {
-			buf.m.fd = planes[0].fd.get();
-		}
-	}
-
-	if (multiPlanar) {
-		buf.length = numV4l2Planes;
-		buf.m.planes = v4l2Planes;
-	}
-
-	if (V4L2_TYPE_IS_OUTPUT(buf.type)) {
-		const FrameMetadata &metadata = buffer->metadata();
-
-		for (const auto &plane : metadata.planes()) {
-			if (!plane.bytesused)
-				LOG(V4L2, Warning) << "byteused == 0 is deprecated";
-		}
-
-		if (numV4l2Planes != planes.size()) {
-			/*
-			 * If we have a multi-planar buffer with a V4L2
-			 * single-planar format, coalesce all planes. The length
-			 * and number of bytes used may only differ in the last
-			 * plane as any other situation can't be represented.
-			 */
-			unsigned int bytesused = 0;
-			unsigned int length = 0;
-
-			for (auto [i, plane] : utils::enumerate(planes)) {
-				bytesused += metadata.planes()[i].bytesused;
-				length += plane.length;
-
-				if (i != planes.size() - 1 && bytesused != length) {
-					LOG(V4L2, Error)
-						<< "Holes in multi-planar buffer not supported";
-					return -EINVAL;
-				}
-			}
-
-			if (multiPlanar) {
-				v4l2Planes[0].bytesused = bytesused;
-				v4l2Planes[0].length = length;
-			} else {
-				buf.bytesused = bytesused;
-				buf.length = length;
-			}
-		} else if (multiPlanar) {
-			/*
-			 * If we use the multi-planar API, fill in the planes.
-			 * The number of planes in the frame buffer and in the
-			 * V4L2 buffer is guaranteed to be equal at this point.
-			 */
-			for (auto [i, plane] : utils::enumerate(planes)) {
-				v4l2Planes[i].bytesused = metadata.planes()[i].bytesused;
-				v4l2Planes[i].length = plane.length;
-			}
-		} else {
-			/*
-			 * Single-planar API with a single plane in the buffer
-			 * is trivial to handle.
-			 */
-			buf.bytesused = metadata.planes()[0].bytesused;
-			buf.length = planes[0].length;
-		}
+	if (queuedBuffers_.size() == VIDEO_MAX_FRAME) {
+		LOG(V4L2, Debug) << "V4L2 queue has " << VIDEO_MAX_FRAME
+				 << " already queued, differing queueing.";
 
-		/*
-		 * Timestamps are to be supplied if the device is a mem-to-mem
-		 * device. The drivers will have V4L2_BUF_FLAG_TIMESTAMP_COPY
-		 * set hence these timestamps will be copied from the output
-		 * buffers to capture buffers. If the device is not mem-to-mem,
-		 * there is no harm in setting the timestamps as they will be
-		 * ignored (and over-written).
-		 */
-		buf.timestamp.tv_sec = metadata.timestamp / 1000000000;
-		buf.timestamp.tv_usec = (metadata.timestamp / 1000) % 1000000;
+		pendingBuffersToQueue_.push(buffer);
+		return 0;
 	}
 
-	LOG(V4L2, Debug) << "Queueing buffer " << buf.index;
+	if (!pendingBuffersToQueue_.empty()) {
+		LOG(V4L2, Debug) << "Adding buffer " << buffer
+				 << " to the pending queue and replacing with "
+				 << pendingBuffersToQueue_.front();
 
-	ret = ioctl(VIDIOC_QBUF, &buf);
-	if (ret < 0) {
-		LOG(V4L2, Error)
-			<< "Failed to queue buffer " << buf.index << ": "
-			<< strerror(-ret);
-		return ret;
-	}
-
-	if (queuedBuffers_.empty()) {
-		fdBufferNotifier_->setEnabled(true);
-		if (watchdogDuration_)
-			watchdog_.start(std::chrono::duration_cast<std::chrono::milliseconds>(watchdogDuration_));
+		pendingBuffersToQueue_.push(buffer);
+		buffer = pendingBuffersToQueue_.front();
+		pendingBuffersToQueue_.pop();
 	}
 
-	queuedBuffers_[buf.index] = buffer;
-
-	return 0;
+	return queueToDevice(buffer);
 }
 
 /**
@@ -1736,6 +1618,11 @@ void V4L2VideoDevice::bufferAvailable()
  * This function dequeues the next available buffer from the device. If no
  * buffer is available to be dequeued it will return nullptr immediately.
  *
+ * Once a buffer is dequeued from the device, this function may also enqueue
+ * a buffer that has been placed in the pending queue (due to reaching the V4L2
+ * queue size limit. Note that if this enqueue fails, we log the error, but
+ * continue running this function to completion.
+ *
  * \return A pointer to the dequeued buffer on success, or nullptr otherwise
  */
 FrameBuffer *V4L2VideoDevice::dequeueBuffer()
@@ -1788,6 +1675,20 @@ FrameBuffer *V4L2VideoDevice::dequeueBuffer()
 	FrameBuffer *buffer = it->second;
 	queuedBuffers_.erase(it);
 
+	if (!pendingBuffersToQueue_.empty()) {
+		FrameBuffer *pending = pendingBuffersToQueue_.front();
+
+		pendingBuffersToQueue_.pop();
+		/*
+		 * If the pending buffer enqueue fails, we must continue this
+		 * function to completion for the dequeue operation.
+		 */
+		if (queueToDevice(pending))
+			LOG(V4L2, Error)
+				<< "Failed to re-queue pending buffer "
+				<< pending;
+	}
+
 	if (queuedBuffers_.empty()) {
 		fdBufferNotifier_->setEnabled(false);
 		watchdog_.stop();
@@ -1887,6 +1788,168 @@ FrameBuffer *V4L2VideoDevice::dequeueBuffer()
 }
 
 /**
+ * \brief Queue a buffer to the video device if possible
+ * \param[in] buffer The buffer to be queued
+ *
+ * For capture video devices the \a buffer will be filled with data by the
+ * device. For output video devices the \a buffer shall contain valid data and
+ * will be processed by the device. Once the device has finished processing the
+ * buffer, it will be available for dequeue.
+ *
+ * The best available V4L2 buffer is picked for \a buffer using the V4L2 buffer
+ * cache.
+ *
+ * Note that queueToDevice() will fail if the device is in the process of being
+ * stopped from a streaming state through streamOff().
+ *
+ * \return 0 on success or a negative error code otherwise
+ */
+int V4L2VideoDevice::queueToDevice(FrameBuffer *buffer)
+{
+	struct v4l2_plane v4l2Planes[VIDEO_MAX_PLANES] = {};
+	struct v4l2_buffer buf = {};
+	int ret;
+
+	/*
+	 * Pipeline handlers should not requeue buffers after releasing the
+	 * buffers on the device. Any occurence of this error should be fixed
+	 * in the pipeline handler directly.
+	 */
+	if (!cache_) {
+		LOG(V4L2, Fatal) << "No BufferCache available to queue.";
+		return -ENOENT;
+	}
+
+	ret = cache_->get(*buffer);
+	if (ret < 0)
+		return ret;
+
+	buf.index = ret;
+	buf.type = bufferType_;
+	buf.memory = memoryType_;
+	buf.field = V4L2_FIELD_NONE;
+
+	bool multiPlanar = V4L2_TYPE_IS_MULTIPLANAR(buf.type);
+	const std::vector<FrameBuffer::Plane> &planes = buffer->planes();
+	const unsigned int numV4l2Planes = format_.planesCount;
+
+	/*
+	 * Ensure that the frame buffer has enough planes, and that they're
+	 * contiguous if the V4L2 format requires them to be.
+	 */
+	if (planes.size() < numV4l2Planes) {
+		LOG(V4L2, Error) << "Frame buffer has too few planes";
+		return -EINVAL;
+	}
+
+	if (planes.size() != numV4l2Planes && !buffer->_d()->isContiguous()) {
+		LOG(V4L2, Error) << "Device format requires contiguous buffer";
+		return -EINVAL;
+	}
+
+	if (buf.memory == V4L2_MEMORY_DMABUF) {
+		if (multiPlanar) {
+			for (unsigned int p = 0; p < numV4l2Planes; ++p)
+				v4l2Planes[p].m.fd = planes[p].fd.get();
+		} else {
+			buf.m.fd = planes[0].fd.get();
+		}
+	}
+
+	if (multiPlanar) {
+		buf.length = numV4l2Planes;
+		buf.m.planes = v4l2Planes;
+	}
+
+	if (V4L2_TYPE_IS_OUTPUT(buf.type)) {
+		const FrameMetadata &metadata = buffer->metadata();
+
+		for (const auto &plane : metadata.planes()) {
+			if (!plane.bytesused)
+				LOG(V4L2, Warning) << "byteused == 0 is deprecated";
+		}
+
+		if (numV4l2Planes != planes.size()) {
+			/*
+			 * If we have a multi-planar buffer with a V4L2
+			 * single-planar format, coalesce all planes. The length
+			 * and number of bytes used may only differ in the last
+			 * plane as any other situation can't be represented.
+			 */
+			unsigned int bytesused = 0;
+			unsigned int length = 0;
+
+			for (auto [i, plane] : utils::enumerate(planes)) {
+				bytesused += metadata.planes()[i].bytesused;
+				length += plane.length;
+
+				if (i != planes.size() - 1 && bytesused != length) {
+					LOG(V4L2, Error)
+						<< "Holes in multi-planar buffer not supported";
+					return -EINVAL;
+				}
+			}
+
+			if (multiPlanar) {
+				v4l2Planes[0].bytesused = bytesused;
+				v4l2Planes[0].length = length;
+			} else {
+				buf.bytesused = bytesused;
+				buf.length = length;
+			}
+		} else if (multiPlanar) {
+			/*
+			 * If we use the multi-planar API, fill in the planes.
+			 * The number of planes in the frame buffer and in the
+			 * V4L2 buffer is guaranteed to be equal at this point.
+			 */
+			for (auto [i, plane] : utils::enumerate(planes)) {
+				v4l2Planes[i].bytesused = metadata.planes()[i].bytesused;
+				v4l2Planes[i].length = plane.length;
+			}
+		} else {
+			/*
+			 * Single-planar API with a single plane in the buffer
+			 * is trivial to handle.
+			 */
+			buf.bytesused = metadata.planes()[0].bytesused;
+			buf.length = planes[0].length;
+		}
+
+		/*
+		 * Timestamps are to be supplied if the device is a mem-to-mem
+		 * device. The drivers will have V4L2_BUF_FLAG_TIMESTAMP_COPY
+		 * set hence these timestamps will be copied from the output
+		 * buffers to capture buffers. If the device is not mem-to-mem,
+		 * there is no harm in setting the timestamps as they will be
+		 * ignored (and over-written).
+		 */
+		buf.timestamp.tv_sec = metadata.timestamp / 1000000000;
+		buf.timestamp.tv_usec = (metadata.timestamp / 1000) % 1000000;
+	}
+
+	LOG(V4L2, Debug) << "Queueing buffer " << buf.index;
+
+	ret = ioctl(VIDIOC_QBUF, &buf);
+	if (ret < 0) {
+		LOG(V4L2, Error)
+			<< "Failed to queue buffer " << buf.index << ": "
+			<< strerror(-ret);
+		return ret;
+	}
+
+	if (queuedBuffers_.empty()) {
+		fdBufferNotifier_->setEnabled(true);
+		if (watchdogDuration_)
+			watchdog_.start(std::chrono::duration_cast<std::chrono::milliseconds>(watchdogDuration_));
+	}
+
+	queuedBuffers_[buf.index] = buffer;
+
+	return 0;
+}
+
+/**
  * \var V4L2VideoDevice::bufferReady
  * \brief A Signal emitted when a framebuffer completes
  */