From 46045316cef28e0d892bc5f319b8ca944f3d51cb Mon Sep 17 00:00:00 2001
From: Wim Taymans <wim.taymans@collabora.co.uk>
Date: Tue, 24 Apr 2012 12:37:33 +0200
Subject: [PATCH] v4l2: use driver timestamps

Use the drive timestamps for timestamping outgoing buffers.
---
 sys/v4l2/gstv4l2bufferpool.c | 12 +++++++---
 sys/v4l2/gstv4l2src.c        | 46 ++++++++++++++++++++++++++++++++++--
 2 files changed, 53 insertions(+), 5 deletions(-)

diff --git a/sys/v4l2/gstv4l2bufferpool.c b/sys/v4l2/gstv4l2bufferpool.c
index 21a5e8f9c6..979877028d 100644
--- a/sys/v4l2/gstv4l2bufferpool.c
+++ b/sys/v4l2/gstv4l2bufferpool.c
@@ -587,6 +587,7 @@ gst_v4l2_buffer_pool_dqbuf (GstV4l2BufferPool * pool, GstBuffer ** buffer)
   GstBuffer *outbuf;
   struct v4l2_buffer vbuffer;
   GstV4l2Object *obj = pool->obj;
+  GstClockTime timestamp;
 
   if (obj->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
     /* select works for input devices when data is available. According to the
@@ -615,10 +616,13 @@ gst_v4l2_buffer_pool_dqbuf (GstV4l2BufferPool * pool, GstBuffer ** buffer)
   pool->buffers[vbuffer.index] = NULL;
   pool->num_queued--;
 
+  timestamp = GST_TIMEVAL_TO_TIME (vbuffer.timestamp);
+
   GST_LOG_OBJECT (pool,
-      "dequeued buffer %p seq:%d (ix=%d), used %d, flags %08x, pool-queued=%d, buffer=%p",
-      outbuf, vbuffer.sequence, vbuffer.index, vbuffer.bytesused, vbuffer.flags,
-      pool->num_queued, outbuf);
+      "dequeued buffer %p seq:%d (ix=%d), used %d, flags %08x, ts %"
+      GST_TIME_FORMAT ", pool-queued=%d, buffer=%p", outbuf, vbuffer.sequence,
+      vbuffer.index, vbuffer.bytesused, vbuffer.flags,
+      GST_TIME_ARGS (timestamp), pool->num_queued, outbuf);
 
   /* set top/bottom field first if v4l2_buffer has the information */
   if (vbuffer.field == V4L2_FIELD_INTERLACED_TB) {
@@ -634,6 +638,8 @@ gst_v4l2_buffer_pool_dqbuf (GstV4l2BufferPool * pool, GstBuffer ** buffer)
   else
     gst_buffer_resize (outbuf, 0, vbuffer.length);
 
+  GST_BUFFER_TIMESTAMP (outbuf) = timestamp;
+
   *buffer = outbuf;
 
   return GST_FLOW_OK;
diff --git a/sys/v4l2/gstv4l2src.c b/sys/v4l2/gstv4l2src.c
index 2f5c812212..026db3cf13 100644
--- a/sys/v4l2/gstv4l2src.c
+++ b/sys/v4l2/gstv4l2src.c
@@ -779,6 +779,7 @@ gst_v4l2src_fill (GstPushSrc * src, GstBuffer * buf)
   GstFlowReturn ret;
   GstClock *clock;
   GstClockTime timestamp, duration;
+  GstClockTime delay;
 
 #if 0
   int i;
@@ -798,6 +799,40 @@ gst_v4l2src_fill (GstPushSrc * src, GstBuffer * buf)
   if (G_UNLIKELY (ret != GST_FLOW_OK))
     goto error;
 
+
+  timestamp = GST_BUFFER_TIMESTAMP (buf);
+
+  if (timestamp != GST_CLOCK_TIME_NONE) {
+    struct timespec now;
+    GstClockTime gstnow;
+
+    /* v4l2 specs say to use the system time although many drivers switched to
+     * the more desirable monotonic time. We first try to use the monotonic time
+     * and see how that goes */
+    clock_gettime (CLOCK_MONOTONIC, &now);
+    gstnow = GST_TIMESPEC_TO_TIME (now);
+
+    if (gstnow < timestamp && (timestamp - gstnow) > (10 * GST_SECOND)) {
+      GTimeVal now;
+
+      /* very large diff, fall back to system time */
+      g_get_current_time (&now);
+      gstnow = GST_TIMEVAL_TO_TIME (now);
+    }
+
+    if (gstnow > timestamp) {
+      delay = gstnow - timestamp;
+    } else {
+      delay = 0;
+    }
+
+    GST_DEBUG_OBJECT (v4l2src, "ts: %" GST_TIME_FORMAT " now %" GST_TIME_FORMAT
+        " delay %" GST_TIME_FORMAT, GST_TIME_ARGS (timestamp),
+        GST_TIME_ARGS (gstnow), GST_TIME_ARGS (delay));
+  } else {
+    delay = 0;
+  }
+
   /* set buffer metadata */
   GST_BUFFER_OFFSET (buf) = v4l2src->offset++;
   GST_BUFFER_OFFSET_END (buf) = v4l2src->offset;
@@ -822,6 +857,12 @@ gst_v4l2src_fill (GstPushSrc * src, GstBuffer * buf)
     timestamp = gst_clock_get_time (clock) - timestamp;
     gst_object_unref (clock);
 
+    /* adjust for delay in the device */
+    if (timestamp > delay)
+      timestamp -= delay;
+    else
+      timestamp = 0;
+
     /* if we have a framerate adjust timestamp for frame latency */
     if (GST_CLOCK_TIME_IS_VALID (duration)) {
       if (timestamp > duration)
@@ -841,8 +882,9 @@ gst_v4l2src_fill (GstPushSrc * src, GstBuffer * buf)
     v4l2src->ctrl_time = timestamp;
   }
   gst_object_sync_values (GST_OBJECT (src), v4l2src->ctrl_time);
-  GST_INFO_OBJECT (src, "sync to %" GST_TIME_FORMAT,
-      GST_TIME_ARGS (v4l2src->ctrl_time));
+
+  GST_INFO_OBJECT (src, "sync to %" GST_TIME_FORMAT " out ts %" GST_TIME_FORMAT,
+      GST_TIME_ARGS (v4l2src->ctrl_time), GST_TIME_ARGS (timestamp));
 
   /* FIXME: use the timestamp from the buffer itself! */
   GST_BUFFER_TIMESTAMP (buf) = timestamp;