v4l2: use dll to track rate difference

Track the difference between the configured and real framerate and use
that as the rate correction to adjust the next_nsec clock field.

spa/plugins/libcamera/libcamera-source.cpp

@@ -19,6 +19,7 @@
 #include <spa/utils/result.h>
 #include <spa/utils/string.h>
 #include <spa/utils/ringbuffer.h>
+#include <spa/utils/dll.h>
 #include <spa/monitor/device.h>
 #include <spa/node/node.h>
 #include <spa/node/io.h>
@@ -167,6 +168,8 @@ struct impl {
 
 	impl(spa_log *log, spa_loop *data_loop, spa_system *system,
 	     std::shared_ptr<CameraManager> manager, std::shared_ptr<Camera> camera, std::string device_id);
+
+	struct spa_dll dll;
 };
 
 }

spa/plugins/libcamera/libcamera-utils.cpp

@@ -931,6 +931,18 @@ void impl::requestComplete(libcamera::Request *request)
 	const FrameMetadata &fmd = buffer->metadata();
 
 	if (impl->clock) {
+		double target = (double)port->info.rate.num / port->info.rate.denom;
+		double corr;
+
+		if (impl->dll.bw == 0.0) {
+			spa_dll_set_bw(&impl->dll, SPA_DLL_BW_MAX, port->info.rate.denom, port->info.rate.denom);
+			impl->clock->next_nsec = fmd.timestamp;
+			corr = 1.0;
+		} else {
+			double diff = ((double)impl->clock->next_nsec - (double)fmd.timestamp) / SPA_NSEC_PER_SEC;
+			double error = port->info.rate.denom * (diff - target);
+			corr = spa_dll_update(&impl->dll, SPA_CLAMPD(error, -128., 128.));
+		}
 		/* FIXME, we should follow the driver clock and target_ values.
 		 * for now we ignore and use our own. */
 		impl->clock->target_rate = port->rate;
@@ -941,8 +953,8 @@ void impl::requestComplete(libcamera::Request *request)
 		impl->clock->position = fmd.sequence;
 		impl->clock->duration = 1;
 		impl->clock->delay = 0;
-		impl->clock->rate_diff = 1.0;
+		impl->clock->rate_diff = corr;
-		impl->clock->next_nsec = fmd.timestamp;
+		impl->clock->next_nsec += (uint64_t) (target * SPA_NSEC_PER_SEC * corr);
 	}
 	if (b->h) {
 		b->h->flags = 0;
@@ -987,6 +999,8 @@ static int spa_libcamera_stream_on(struct impl *impl)
 	}
 	impl->pendingRequests.clear();
 
+	impl->dll.bw = 0.0;
+
 	impl->source.func = libcamera_on_fd_events;
 	impl->source.data = impl;
 	impl->source.fd = spa_system_eventfd_create(impl->system, SPA_FD_CLOEXEC | SPA_FD_NONBLOCK);

spa/plugins/v4l2/meson.build

@@ -1,7 +1,7 @@
 v4l2_sources = ['v4l2.c',
                 'v4l2-device.c',
                 'v4l2-source.c']
-v4l2_dependencies = [ spa_dep, libinotify_dep ]
+v4l2_dependencies = [ spa_dep, libinotify_dep, mathlib ]
 
 if libudev_dep.found()
   v4l2_sources += [ 'v4l2-udev.c' ]

spa/plugins/v4l2/v4l2-source.c

@@ -16,6 +16,7 @@
 #include <spa/utils/keys.h>
 #include <spa/utils/names.h>
 #include <spa/utils/string.h>
+#include <spa/utils/dll.h>
 #include <spa/monitor/device.h>
 #include <spa/node/node.h>
 #include <spa/node/io.h>
@@ -147,6 +148,8 @@ struct impl {
 	struct spa_io_clock *clock;
 
 	struct spa_latency_info latency[2];
+
+	struct spa_dll dll;
 };
 
 #define CHECK_PORT(this,direction,port_id)  ((direction) == SPA_DIRECTION_OUTPUT && (port_id) == 0)

spa/plugins/v4l2/v4l2-utils.c

@@ -1429,7 +1429,21 @@ static int mmap_read(struct impl *this)
 
 	pts = SPA_TIMEVAL_TO_NSEC(&buf.timestamp);
 
+
 	if (this->clock) {
+		double target = (double)port->info.rate.num / port->info.rate.denom;
+		double corr;
+
+		if (this->dll.bw == 0.0) {
+			spa_dll_set_bw(&this->dll, SPA_DLL_BW_MAX, port->info.rate.denom, port->info.rate.denom);
+			this->clock->next_nsec = pts;
+			corr = 1.0;
+		} else {
+			double diff = ((double)this->clock->next_nsec - (double)pts) / SPA_NSEC_PER_SEC;
+			double error = port->info.rate.denom * (diff - target);
+			corr = spa_dll_update(&this->dll, SPA_CLAMPD(error, -128., 128.));
+		}
+
 		/* FIXME, we should follow the driver clock and target_ values.
 		 * for now we ignore and use our own. */
 		this->clock->target_rate = port->info.rate;
@@ -1440,8 +1454,8 @@ static int mmap_read(struct impl *this)
 		this->clock->position = buf.sequence;
 		this->clock->duration = 1;
 		this->clock->delay = 0;
-		this->clock->rate_diff = 1.0;
+		this->clock->rate_diff = corr;
-		this->clock->next_nsec = pts + port->info.rate.num * SPA_NSEC_PER_SEC / port->info.rate.denom;
+		this->clock->next_nsec += (uint64_t) (target * SPA_NSEC_PER_SEC * corr);
 	}
 
 	b = &port->buffers[buf.index];
@@ -1856,6 +1870,7 @@ static int spa_v4l2_stream_on(struct impl *this)
 		spa_log_error(this->log, "'%s' VIDIOC_STREAMON: %m", this->props.device);
 		return -errno;
 	}
+	this->dll.bw = 0.0;
 
 	port->source.func = v4l2_on_fd_events;
 	port->source.data = this;