diff --git a/spa/plugins/libcamera/libcamera-source.cpp b/spa/plugins/libcamera/libcamera-source.cpp index 5e26252b8..f570499d1 100644 --- a/spa/plugins/libcamera/libcamera-source.cpp +++ b/spa/plugins/libcamera/libcamera-source.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -167,6 +168,8 @@ struct impl { impl(spa_log *log, spa_loop *data_loop, spa_system *system, std::shared_ptr manager, std::shared_ptr camera, std::string device_id); + + struct spa_dll dll; }; } diff --git a/spa/plugins/libcamera/libcamera-utils.cpp b/spa/plugins/libcamera/libcamera-utils.cpp index 7f8f186bc..92185c4b0 100644 --- a/spa/plugins/libcamera/libcamera-utils.cpp +++ b/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->next_nsec = fmd.timestamp; + impl->clock->rate_diff = corr; + 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); diff --git a/spa/plugins/v4l2/meson.build b/spa/plugins/v4l2/meson.build index e7d09fe2a..22746a161 100644 --- a/spa/plugins/v4l2/meson.build +++ b/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' ] diff --git a/spa/plugins/v4l2/v4l2-source.c b/spa/plugins/v4l2/v4l2-source.c index 6a6a0830f..6fd62c8bf 100644 --- a/spa/plugins/v4l2/v4l2-source.c +++ b/spa/plugins/v4l2/v4l2-source.c @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -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) diff --git a/spa/plugins/v4l2/v4l2-utils.c b/spa/plugins/v4l2/v4l2-utils.c index 91ba4b2c2..724e72913 100644 --- a/spa/plugins/v4l2/v4l2-utils.c +++ b/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->next_nsec = pts + port->info.rate.num * SPA_NSEC_PER_SEC / port->info.rate.denom; + this->clock->rate_diff = corr; + 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;