2 files changed, 75 insertions, 14 deletions

diff --git a/src/libcamera/pipeline/raspberrypi/rpi_stream.cpp b/src/libcamera/pipeline/raspberrypi/rpi_stream.cpp
index b4d737db..b2a5dfa7 100644
--- a/src/libcamera/pipeline/raspberrypi/rpi_stream.cpp
+++ b/src/libcamera/pipeline/raspberrypi/rpi_stream.cpp
@@ -94,38 +94,75 @@ int RPiStream::queueBuffer(FrameBuffer *buffer)
 {
 	/*
 	 * A nullptr buffer implies an external stream, but no external
-	 * buffer has been supplied. So, pick one from the availableBuffers_
-	 * queue.
+	 * buffer has been supplied in the Request. So, pick one from the
+	 * availableBuffers_ queue.
 	 */
 	if (!buffer) {
 		if (availableBuffers_.empty()) {
-			LOG(RPISTREAM, Warning) << "No buffers available for "
+			LOG(RPISTREAM, Info) << "No buffers available for "
 						<< name_;
-			return -EINVAL;
+			/*
+			 * Note that we need to queue an internal buffer as soon
+			 * as one becomes available.
+			 */
+			requestBuffers_.push(nullptr);
+			return 0;
 		}
 
 		buffer = availableBuffers_.front();
 		availableBuffers_.pop();
 	}
 
-	LOG(RPISTREAM, Debug) << "Queuing buffer " << buffer->cookie()
-			      << " for " << name_;
+	/*
+	 * If no earlier requests are pending to be queued we can go ahead and
+	 * queue this buffer into the device.
+	 */
+	if (requestBuffers_.empty())
+		return queueToDevice(buffer);
 
-	int ret = dev_->queueBuffer(buffer);
-	if (ret) {
-		LOG(RPISTREAM, Error) << "Failed to queue buffer for "
-				      << name_;
-	}
+	/*
+	 * There are earlier Request buffers to be queued, so this buffer must go
+	 * on the waiting list.
+	 */
+	requestBuffers_.push(buffer);
 
-	return ret;
+	return 0;
 }
 
 void RPiStream::returnBuffer(FrameBuffer *buffer)
 {
 	/* This can only be called for external streams. */
-	assert(external_);
+	ASSERT(external_);
 
+	/* Push this buffer back into the queue to be used again. */
 	availableBuffers_.push(buffer);
+
+	/*
+	 * Do we have any Request buffers that are waiting to be queued?
+	 * If so, do it now as availableBuffers_ will not be empty.
+	 */
+	while (!requestBuffers_.empty()) {
+		FrameBuffer *buffer = requestBuffers_.front();
+
+		if (!buffer) {
+			/*
+			 * We want to queue an internal buffer, but none
+			 * are available. Can't do anything, quit the loop.
+			 */
+			if (availableBuffers_.empty())
+				break;
+
+			/*
+			 * We want to queue an internal buffer, and at least one
+			 * is available.
+			 */
+			buffer = availableBuffers_.front();
+			availableBuffers_.pop();
+		}
+
+		requestBuffers_.pop();
+		queueToDevice(buffer);
+	}
 }
 
 int RPiStream::queueAllBuffers()
@@ -155,10 +192,23 @@ void RPiStream::releaseBuffers()
 void RPiStream::clearBuffers()
 {
 	availableBuffers_ = std::queue<FrameBuffer *>{};
+	requestBuffers_ = std::queue<FrameBuffer *>{};
 	internalBuffers_.clear();
 	bufferList_.clear();
 }
 
+int RPiStream::queueToDevice(FrameBuffer *buffer)
+{
+	LOG(RPISTREAM, Debug) << "Queuing buffer " << buffer->cookie()
+			      << " for " << name_;
+
+	int ret = dev_->queueBuffer(buffer);
+	if (ret)
+		LOG(RPISTREAM, Error) << "Failed to queue buffer for "
+				      << name_;
+	return ret;
+}
+
 } /* namespace RPi */
 
 } /* namespace libcamera */
diff --git a/src/libcamera/pipeline/raspberrypi/rpi_stream.h b/src/libcamera/pipeline/raspberrypi/rpi_stream.h
index 73954ec7..f42e25b9 100644
--- a/src/libcamera/pipeline/raspberrypi/rpi_stream.h
+++ b/src/libcamera/pipeline/raspberrypi/rpi_stream.h
@@ -57,6 +57,7 @@ public:
 
 private:
 	void clearBuffers();
+	int queueToDevice(FrameBuffer *buffer);
 
 	/*
 	 * Indicates that this stream is active externally, i.e. the buffers
@@ -73,7 +74,7 @@ private:
 	/* The actual device stream. */
 	std::unique_ptr<V4L2VideoDevice> dev_;
 
-	/* All framebuffers associated with this device stream. */
+	/* All frame buffers associated with this device stream. */
 	std::vector<FrameBuffer *> bufferList_;
 
 	/*
@@ -84,6 +85,16 @@ private:
 	std::queue<FrameBuffer *> availableBuffers_;
 
 	/*
+	 * List of frame buffers that are to be queued into the device from a Request.
+	 * A nullptr indicates any internal buffer can be used (from availableBuffers_),
+	 * whereas a valid pointer indicates an external buffer to be queued.
+	 *
+	 * Ordering buffers to be queued is important here as it must match the
+	 * requests coming from the application.
+	 */
+	std::queue<FrameBuffer *> requestBuffers_;
+
+	/*
 	 * This is a list of buffers exported internally. Need to keep this around
 	 * as the stream needs to maintain ownership of these buffers.
 	 */