[PATCH 2/2] usb: gadget: uvc: implement dwPresentationTime and scrSourceClock

[Michael Grzeschik <m.grzeschik@xxxxxxxxxxxxxx>]

From: Michael Olbrich <m.olbrich@xxxxxxxxxxxxxx>

This patch adds the fields UVC_STREAM_PTS and UVC_STREAM_SCR to the uvc
header, in case this data is available. It also enables the copy of the
timestamp to the vb2_v4l2_buffer by setting V4L2_BUF_FLAG_TIMESTAMP_COPY
in the queue.timestamp_flags.

Signed-off-by: Michael Olbrich <m.olbrich@xxxxxxxxxxxxxx>
Signed-off-by: Michael Grzeschik <m.grzeschik@xxxxxxxxxxxxxx>

---
 drivers/usb/gadget/function/uvc.h       |  2 +-
 drivers/usb/gadget/function/uvc_queue.c |  2 +-
 drivers/usb/gadget/function/uvc_video.c | 35 ++++++++++++++++++++++---
 3 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/drivers/usb/gadget/function/uvc.h b/drivers/usb/gadget/function/uvc.h
index b05de36e2c605..c3607a32b9862 100644
--- a/drivers/usb/gadget/function/uvc.h
+++ b/drivers/usb/gadget/function/uvc.h
@@ -68,7 +68,7 @@ extern unsigned int uvc_gadget_trace_param;
 #define UVC_MAX_REQUEST_SIZE			64
 #define UVC_MAX_EVENTS				4
 
-#define UVCG_REQUEST_HEADER_LEN			2
+#define UVCG_REQUEST_HEADER_LEN			12
 
 /* ------------------------------------------------------------------------
  * Structures
diff --git a/drivers/usb/gadget/function/uvc_queue.c b/drivers/usb/gadget/function/uvc_queue.c
index 7d00ad7c154c2..d852ac9e47e72 100644
--- a/drivers/usb/gadget/function/uvc_queue.c
+++ b/drivers/usb/gadget/function/uvc_queue.c
@@ -142,7 +142,7 @@ int uvcg_queue_init(struct uvc_video_queue *queue, struct device *dev, enum v4l2
 		queue->queue.mem_ops = &vb2_vmalloc_memops;
 	}
 
-	queue->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
+	queue->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY
 				     | V4L2_BUF_FLAG_TSTAMP_SRC_EOF;
 	queue->queue.dev = dev;
 
diff --git a/drivers/usb/gadget/function/uvc_video.c b/drivers/usb/gadget/function/uvc_video.c
index 2a9ab54bb3dc3..7f59a0c474020 100644
--- a/drivers/usb/gadget/function/uvc_video.c
+++ b/drivers/usb/gadget/function/uvc_video.c
@@ -12,6 +12,7 @@
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/video.h>
+#include <asm/unaligned.h>
 
 #include <media/v4l2-dev.h>
 
@@ -27,13 +28,41 @@ static int
 uvc_video_encode_header(struct uvc_video *video, struct uvc_buffer *buf,
 		u8 *data, int len)
 {
-	data[0] = UVCG_REQUEST_HEADER_LEN;
+	struct uvc_device *uvc = container_of(video, struct uvc_device, video);
+	struct usb_composite_dev *cdev = uvc->func.config->cdev;
+	struct timespec64 ts = ns_to_timespec64(buf->buf.vb2_buf.timestamp);
+	int pos = 2;
+
 	data[1] = UVC_STREAM_EOH | video->fid;
 
-	if (buf->bytesused - video->queue.buf_used <= len - UVCG_REQUEST_HEADER_LEN)
+	if (video->queue.buf_used == 0 && ts.tv_sec) {
+		/* dwClockFrequency is 48 MHz */
+		u32 pts = ((u64)ts.tv_sec * USEC_PER_SEC + ts.tv_nsec / NSEC_PER_USEC) * 48;
+
+		data[1] |= UVC_STREAM_PTS;
+		put_unaligned_le32(pts, &data[pos]);
+		pos += 4;
+	}
+
+	if (cdev->gadget->ops->get_frame) {
+		u32 sof, stc;
+
+		sof = usb_gadget_frame_number(cdev->gadget);
+		ktime_get_ts64(&ts);
+		stc = ((u64)ts.tv_sec * USEC_PER_SEC + ts.tv_nsec / NSEC_PER_USEC) * 48;
+
+		data[1] |= UVC_STREAM_SCR;
+		put_unaligned_le32(stc, &data[pos]);
+		put_unaligned_le16(sof, &data[pos+4]);
+		pos += 6;
+	}
+
+	data[0] = pos;
+
+	if (buf->bytesused - video->queue.buf_used <= len - pos)
 		data[1] |= UVC_STREAM_EOF;
 
-	return UVCG_REQUEST_HEADER_LEN;
+	return pos;
 }
 
 static int
-- 
2.30.2