[RFC PATCH 3/3] libcamera: v4l2: Add wallclock metadata to video devices

[David Plowman]

FrameMetadata is extended to include wallclock timestamps, both
"raw" (unsmoothed) and de-jittered versions.

The "raw" wallclock timestamps are recorded when frames start, and at
frame end we generate a de-jittered version of this wallclock, handing
both out to pipeline handlers.

Signed-off-by: David Plowman <david.plowman at raspberrypi.com>
---
 include/libcamera/framebuffer.h               |  2 ++
 include/libcamera/internal/v4l2_device.h      |  4 +++
 include/libcamera/internal/v4l2_videodevice.h |  3 ++
 src/libcamera/v4l2_device.cpp                 | 22 ++++++++++++++
 src/libcamera/v4l2_videodevice.cpp            | 29 +++++++++++++++++++
 5 files changed, 60 insertions(+)

diff --git a/include/libcamera/framebuffer.h b/include/libcamera/framebuffer.h
index ff839243..a181d97a 100644
--- a/include/libcamera/framebuffer.h
+++ b/include/libcamera/framebuffer.h
@@ -35,6 +35,8 @@ struct FrameMetadata {
 	Status status;
 	unsigned int sequence;
 	uint64_t timestamp;
+	uint64_t wallClockRaw;
+	uint64_t wallClock;
 
 	Span<Plane> planes() { return planes_; }
 	Span<const Plane> planes() const { return planes_; }
diff --git a/include/libcamera/internal/v4l2_device.h b/include/libcamera/internal/v4l2_device.h
index f5aa5024..90dbc4a8 100644
--- a/include/libcamera/internal/v4l2_device.h
+++ b/include/libcamera/internal/v4l2_device.h
@@ -10,6 +10,7 @@
 #include <map>
 #include <memory>
 #include <optional>
+#include <queue>
 #include <vector>
 
 #include <linux/videodev2.h>
@@ -67,6 +68,9 @@ protected:
 	template<typename T>
 	static int fromColorSpace(const std::optional<ColorSpace> &colorSpace, T &v4l2Format);
 
+	std::queue<std::pair<uint64_t, uint64_t>> wallClockQueue_;
+	bool frameStartEnabled() const { return frameStartEnabled_; }
+
 private:
 	static ControlType v4l2CtrlType(uint32_t ctrlType);
 	static std::unique_ptr<ControlId> v4l2ControlId(const v4l2_query_ext_ctrl &ctrl);
diff --git a/include/libcamera/internal/v4l2_videodevice.h b/include/libcamera/internal/v4l2_videodevice.h
index f021c2a0..d6127928 100644
--- a/include/libcamera/internal/v4l2_videodevice.h
+++ b/include/libcamera/internal/v4l2_videodevice.h
@@ -31,6 +31,7 @@
 #include <libcamera/geometry.h>
 #include <libcamera/pixel_format.h>
 
+#include "libcamera/internal/clock_recovery.h"
 #include "libcamera/internal/formats.h"
 #include "libcamera/internal/v4l2_device.h"
 #include "libcamera/internal/v4l2_pixelformat.h"
@@ -290,6 +291,8 @@ private:
 
 	Timer watchdog_;
 	utils::Duration watchdogDuration_;
+
+	ClockRecovery wallClockRecovery_;
 };
 
 class V4L2M2MDevice
diff --git a/src/libcamera/v4l2_device.cpp b/src/libcamera/v4l2_device.cpp
index 7d21cf15..285527b4 100644
--- a/src/libcamera/v4l2_device.cpp
+++ b/src/libcamera/v4l2_device.cpp
@@ -474,6 +474,11 @@ int V4L2Device::ioctl(unsigned long request, void *argp)
  * \return The V4L2 device file descriptor, -1 if the device node is not open
  */
 
+/**
+ * \fn V4L2Device::frameStartEnabled()
+ * \return True if frame start notifications are enabled, otherwise false
+ */
+
 /**
  * \brief Retrieve the libcamera control type associated with the V4L2 control
  * \param[in] ctrlType The V4L2 control type
@@ -761,6 +766,23 @@ void V4L2Device::eventAvailable()
 		return;
 	}
 
+	/*
+	 * Record this frame (by its sequence number) and its corresponding wallclock value.
+	 * Use a queue as these two events may not interleave perfectly.
+	 */
+	auto now = std::chrono::system_clock::now();
+	uint64_t wallClock = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
+
+	wallClockQueue_.emplace(event.u.frame_sync.frame_sequence, wallClock);
+
+	/*
+	 * Also avoid growing the queue indefiniteily. It seems highly unlikely that you could
+	 * get more than a few "frame starts" being processed without a "frame end", so the value
+	 * 5, while arbitrary, appears to be more than enough in practice.
+	 */
+	while (wallClockQueue_.size() > 5)
+		wallClockQueue_.pop();
+
 	frameStart.emit(event.u.frame_sync.frame_sequence);
 }
 
diff --git a/src/libcamera/v4l2_videodevice.cpp b/src/libcamera/v4l2_videodevice.cpp
index 14eba056..5c5dfc79 100644
--- a/src/libcamera/v4l2_videodevice.cpp
+++ b/src/libcamera/v4l2_videodevice.cpp
@@ -1865,6 +1865,33 @@ FrameBuffer *V4L2VideoDevice::dequeueBuffer()
 	metadata.timestamp = buf.timestamp.tv_sec * 1000000000ULL
 			   + buf.timestamp.tv_usec * 1000ULL;
 
+	if (frameStartEnabled()) {
+		/*
+		 * Find the wallclock that should have been recorded for this frame, discarding any
+		 * stale frames on the way.
+		 */
+		while (!wallClockQueue_.empty() && wallClockQueue_.front().first < buf.sequence)
+			wallClockQueue_.pop();
+
+		if (!wallClockQueue_.empty() && wallClockQueue_.front().first == buf.sequence) {
+			metadata.wallClockRaw = wallClockQueue_.front().second;
+			wallClockQueue_.pop();
+		} else {
+			/*
+			 * At higher framerates it can happen that this gets handled before the frame
+			 * start event, meaning there's no wallclock time in the queue. So the best we
+			 * can do is sample the wallclock now. (The frame start will subsequently add
+			 * another wallclock timestamp, but this will get safely discarded.)
+			 */
+			auto now = std::chrono::system_clock::now();
+			metadata.wallClockRaw = std::chrono::duration_cast<std::chrono::microseconds>(now.time_since_epoch()).count();
+		}
+
+		/* Now recover a de-jittered wallclock value. Everything must be in microseconds. */
+		wallClockRecovery_.addSample(metadata.timestamp / 1000, metadata.wallClockRaw);
+		metadata.wallClock = wallClockRecovery_.getOutput(metadata.timestamp / 1000);
+	}
+
 	if (V4L2_TYPE_IS_OUTPUT(buf.type))
 		return buffer;
 
@@ -1958,6 +1985,8 @@ int V4L2VideoDevice::streamOn()
 
 	firstFrame_.reset();
 
+	wallClockQueue_ = {};
+
 	ret = ioctl(VIDIOC_STREAMON, &bufferType_);
 	if (ret < 0) {
 		LOG(V4L2, Error)
-- 
2.39.5