fix: 不能“sender 里先按 fps 睡再去 DQBUF”

cmd/b_side_video_sender.c

@@ -285,6 +285,7 @@ static int open_v4l2_device(const char *device) {
 
 static int init_v4l2_device(int fd, const worker_config_t *cfg) {
     struct v4l2_format fmt;
+    struct v4l2_streamparm parm;
 
     CLEAR(fmt);
     fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
@@ -297,6 +298,18 @@ static int init_v4l2_device(int fd, const worker_config_t *cfg) {
         perror("VIDIOC_S_FMT");
         return -1;
     }
+
+    CLEAR(parm);
+    parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+    if (cfg->initial_fps > 0 && ioctl(fd, VIDIOC_G_PARM, &parm) == 0) {
+        if ((parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) != 0U) {
+            parm.parm.capture.timeperframe.numerator = 1U;
+            parm.parm.capture.timeperframe.denominator = (unsigned int) cfg->initial_fps;
+            if (ioctl(fd, VIDIOC_S_PARM, &parm) < 0) {
+                perror("VIDIOC_S_PARM");
+            }
+        }
+    }
     return 0;
 }
 
@@ -359,6 +372,58 @@ static int queue_all_buffers(int fd, int num_buffers) {
     return 0;
 }
 
+static int dequeue_latest_buffer(int fd, struct v4l2_buffer *latest_buf) {
+    struct v4l2_buffer latest_local;
+    bool have_latest = false;
+
+    if (latest_buf == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    for (;;) {
+        struct v4l2_buffer current;
+        int dq_errno;
+
+        CLEAR(current);
+        current.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+        current.memory = V4L2_MEMORY_MMAP;
+        if (ioctl(fd, VIDIOC_DQBUF, &current) < 0) {
+            dq_errno = errno;
+            if (dq_errno == EINTR) {
+                continue;
+            }
+            if (dq_errno == EAGAIN) {
+                if (!have_latest) {
+                    errno = EAGAIN;
+                    return 1;
+                }
+                *latest_buf = latest_local;
+                return 0;
+            }
+            if (have_latest && ioctl(fd, VIDIOC_QBUF, &latest_local) < 0) {
+                perror("VIDIOC_QBUF");
+            }
+            errno = dq_errno;
+            return -1;
+        }
+
+        if (have_latest && ioctl(fd, VIDIOC_QBUF, &latest_local) < 0) {
+            int q_errno = errno;
+
+            perror("VIDIOC_QBUF");
+            if (ioctl(fd, VIDIOC_QBUF, &current) < 0) {
+                perror("VIDIOC_QBUF");
+            }
+            errno = q_errno;
+            return -1;
+        }
+
+        latest_local = current;
+        have_latest = true;
+    }
+}
+
 static AVCodecContext *create_mjpeg_decoder(const worker_config_t *cfg) {
     const AVCodec *decoder = avcodec_find_decoder(AV_CODEC_ID_MJPEG);
     AVCodecContext *ctx;
@@ -715,10 +780,6 @@ int main(void) {
         if (fps < 1) {
             fps = 1;
         }
-        if (next_deadline_ms > now_ms) {
-            usleep((useconds_t) ((next_deadline_ms - now_ms) * 1000.0));
-        }
-        next_deadline_ms = monotonic_ms() + (1000.0 / (double) fps);
 
         FD_ZERO(&fds);
         FD_SET(camera_fd, &fds);
@@ -733,10 +794,7 @@ int main(void) {
             continue;
         }
 
-        CLEAR(buf);
-        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-        buf.memory = V4L2_MEMORY_MMAP;
-        if (ioctl(camera_fd, VIDIOC_DQBUF, &buf) < 0) {
+        if (dequeue_latest_buffer(camera_fd, &buf) != 0) {
             if (errno == EAGAIN) {
                 continue;
             }
@@ -744,6 +802,13 @@ int main(void) {
             break;
         }
 
+        now_ms = monotonic_ms();
+        if (now_ms < next_deadline_ms) {
+            drop_reason = "paced_drop";
+            goto requeue_and_report;
+        }
+        next_deadline_ms = now_ms + (1000.0 / (double) fps);
+
         if (decode_mjpeg_frame(decoder, (uint8_t *) buffers[buf.index].start, (int) buf.bytesused, &decoded_frame) != 0) {
             drop_reason = "decode_failed";
             goto requeue_and_report;