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

[David Plowman]

FrameMetadata is extended to include a wallclock timestamp.

When a frame is dequeued, we use the ClockRecovery class to generate a
good estimate of a wallclock timestamp to correspond to the frame's
SensorTimestamp.

Pipeline handlers must enable wallclocks for chosen devices by passing
an appropriate ClockRecovery object.

Signed-off-by: David Plowman <david.plowman at raspberrypi.com>
---
 include/libcamera/framebuffer.h               |  1 +
 include/libcamera/internal/v4l2_videodevice.h |  5 +++
 src/libcamera/v4l2_videodevice.cpp            | 36 ++++++++++++++++++-
 3 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/include/libcamera/framebuffer.h b/include/libcamera/framebuffer.h
index ff839243..4579d0c6 100644
--- a/include/libcamera/framebuffer.h
+++ b/include/libcamera/framebuffer.h
@@ -35,6 +35,7 @@ struct FrameMetadata {
 	Status status;
 	unsigned int sequence;
 	uint64_t timestamp;
+	uint64_t wallClock;
 
 	Span<Plane> planes() { return planes_; }
 	Span<const Plane> planes() const { return planes_; }
diff --git a/include/libcamera/internal/v4l2_videodevice.h b/include/libcamera/internal/v4l2_videodevice.h
index f021c2a0..5327c112 100644
--- a/include/libcamera/internal/v4l2_videodevice.h
+++ b/include/libcamera/internal/v4l2_videodevice.h
@@ -37,6 +37,7 @@
 
 namespace libcamera {
 
+class ClockRecovery;
 class EventNotifier;
 class MediaDevice;
 class MediaEntity;
@@ -232,6 +233,8 @@ public:
 
 	V4L2PixelFormat toV4L2PixelFormat(const PixelFormat &pixelFormat) const;
 
+	void enableWallClock(ClockRecovery *wallClockRecovery);
+
 protected:
 	std::string logPrefix() const override;
 
@@ -290,6 +293,8 @@ private:
 
 	Timer watchdog_;
 	utils::Duration watchdogDuration_;
+
+	ClockRecovery *wallClockRecovery_;
 };
 
 class V4L2M2MDevice
diff --git a/src/libcamera/v4l2_videodevice.cpp b/src/libcamera/v4l2_videodevice.cpp
index a5cf6784..2007dffc 100644
--- a/src/libcamera/v4l2_videodevice.cpp
+++ b/src/libcamera/v4l2_videodevice.cpp
@@ -26,6 +26,7 @@
 #include <libcamera/base/unique_fd.h>
 #include <libcamera/base/utils.h>
 
+#include "libcamera/internal/clock_recovery.h"
 #include "libcamera/internal/formats.h"
 #include "libcamera/internal/framebuffer.h"
 #include "libcamera/internal/media_device.h"
@@ -534,7 +535,7 @@ std::ostream &operator<<(std::ostream &out, const V4L2DeviceFormat &f)
 V4L2VideoDevice::V4L2VideoDevice(const std::string &deviceNode)
 	: V4L2Device(deviceNode), formatInfo_(nullptr), cache_(nullptr),
 	  fdBufferNotifier_(nullptr), state_(State::Stopped),
-	  watchdogDuration_(0.0)
+	  watchdogDuration_(0.0), wallClockRecovery_(nullptr)
 {
 	/*
 	 * We default to an MMAP based CAPTURE video device, however this will
@@ -1865,6 +1866,17 @@ FrameBuffer *V4L2VideoDevice::dequeueBuffer()
 	metadata.timestamp = buf.timestamp.tv_sec * 1000000000ULL
 			   + buf.timestamp.tv_usec * 1000ULL;
 
+	metadata.wallClock = 0;
+	if (wallClockRecovery_) {
+		/*
+		 * Sample the internal (CLOCK_BOOTTIME) and realtime (CLOCK_REALTIME) clocks and
+		 * update the clock recovery model. Then derive the wallclock estimate for the
+		 * frame timestamp.
+		 */
+		wallClockRecovery_->addSample();
+		metadata.wallClock = wallClockRecovery_->getOutput(metadata.timestamp / 1000);
+	}
+
 	if (V4L2_TYPE_IS_OUTPUT(buf.type))
 		return buffer;
 
@@ -1965,6 +1977,11 @@ int V4L2VideoDevice::streamOn()
 		return ret;
 	}
 
+	if (wallClockRecovery_) {
+		/* A good moment to sample the clocks to improve the clock recovery model. */
+		wallClockRecovery_->addSample();
+	}
+
 	state_ = State::Streaming;
 	if (watchdogDuration_ && !queuedBuffers_.empty())
 		watchdog_.start(std::chrono::duration_cast<std::chrono::milliseconds>(watchdogDuration_));
@@ -2120,6 +2137,23 @@ V4L2PixelFormat V4L2VideoDevice::toV4L2PixelFormat(const PixelFormat &pixelForma
 	return {};
 }
 
+/**
+ * \brief Enable wall clock timestamps for this device
+ * \param[in] wallClockRecovery an appropriately configured ClockRecovery, or
+ * nullptr to disable wall clocks
+ *
+ * When buffers are dequeued, wall clock timestamps will be generated that
+ * correspond to the frame's timestamp, as returned by V4l2.
+ */
+void V4L2VideoDevice::enableWallClock(ClockRecovery *wallClockRecovery)
+{
+	wallClockRecovery_ = wallClockRecovery;
+
+	/* Also a reasonable moment to sample the two clocks. */
+	if (wallClockRecovery_)
+		wallClockRecovery_->addSample();
+}
+
 /**
  * \class V4L2M2MDevice
  * \brief Memory-to-Memory video device
-- 
2.39.5