diff options
Diffstat (limited to 'drivers/media/usb/uvc/uvc_video.c')
| -rw-r--r-- | drivers/media/usb/uvc/uvc_video.c | 1879 |
1 files changed, 1879 insertions, 0 deletions
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c new file mode 100644 index 00000000000..1c15b4227bd --- /dev/null +++ b/drivers/media/usb/uvc/uvc_video.c @@ -0,0 +1,1879 @@ +/* + * uvc_video.c -- USB Video Class driver - Video handling + * + * Copyright (C) 2005-2010 + * Laurent Pinchart (laurent.pinchart@ideasonboard.com) + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + */ + +#include <linux/kernel.h> +#include <linux/list.h> +#include <linux/module.h> +#include <linux/slab.h> +#include <linux/usb.h> +#include <linux/videodev2.h> +#include <linux/vmalloc.h> +#include <linux/wait.h> +#include <linux/atomic.h> +#include <asm/unaligned.h> + +#include <media/v4l2-common.h> + +#include "uvcvideo.h" + +/* ------------------------------------------------------------------------ + * UVC Controls + */ + +static int __uvc_query_ctrl(struct uvc_device *dev, __u8 query, __u8 unit, + __u8 intfnum, __u8 cs, void *data, __u16 size, + int timeout) +{ + __u8 type = USB_TYPE_CLASS | USB_RECIP_INTERFACE; + unsigned int pipe; + + pipe = (query & 0x80) ? usb_rcvctrlpipe(dev->udev, 0) + : usb_sndctrlpipe(dev->udev, 0); + type |= (query & 0x80) ? USB_DIR_IN : USB_DIR_OUT; + + return usb_control_msg(dev->udev, pipe, query, type, cs << 8, + unit << 8 | intfnum, data, size, timeout); +} + +static const char *uvc_query_name(__u8 query) +{ + switch (query) { + case UVC_SET_CUR: + return "SET_CUR"; + case UVC_GET_CUR: + return "GET_CUR"; + case UVC_GET_MIN: + return "GET_MIN"; + case UVC_GET_MAX: + return "GET_MAX"; + case UVC_GET_RES: + return "GET_RES"; + case UVC_GET_LEN: + return "GET_LEN"; + case UVC_GET_INFO: + return "GET_INFO"; + case UVC_GET_DEF: + return "GET_DEF"; + default: + return "<invalid>"; + } +} + +int uvc_query_ctrl(struct uvc_device *dev, __u8 query, __u8 unit, + __u8 intfnum, __u8 cs, void *data, __u16 size) +{ + int ret; + + ret = __uvc_query_ctrl(dev, query, unit, intfnum, cs, data, size, + UVC_CTRL_CONTROL_TIMEOUT); + if (ret != size) { + uvc_printk(KERN_ERR, "Failed to query (%s) UVC control %u on " + "unit %u: %d (exp. %u).\n", uvc_query_name(query), cs, + unit, ret, size); + return -EIO; + } + + return 0; +} + +static void uvc_fixup_video_ctrl(struct uvc_streaming *stream, + struct uvc_streaming_control *ctrl) +{ + struct uvc_format *format = NULL; + struct uvc_frame *frame = NULL; + unsigned int i; + + for (i = 0; i < stream->nformats; ++i) { + if (stream->format[i].index == ctrl->bFormatIndex) { + format = &stream->format[i]; + break; + } + } + + if (format == NULL) + return; + + for (i = 0; i < format->nframes; ++i) { + if (format->frame[i].bFrameIndex == ctrl->bFrameIndex) { + frame = &format->frame[i]; + break; + } + } + + if (frame == NULL) + return; + + if (!(format->flags & UVC_FMT_FLAG_COMPRESSED) || + (ctrl->dwMaxVideoFrameSize == 0 && + stream->dev->uvc_version < 0x0110)) + ctrl->dwMaxVideoFrameSize = + frame->dwMaxVideoFrameBufferSize; + + if (!(format->flags & UVC_FMT_FLAG_COMPRESSED) && + stream->dev->quirks & UVC_QUIRK_FIX_BANDWIDTH && + stream->intf->num_altsetting > 1) { + u32 interval; + u32 bandwidth; + + interval = (ctrl->dwFrameInterval > 100000) + ? ctrl->dwFrameInterval + : frame->dwFrameInterval[0]; + + /* Compute a bandwidth estimation by multiplying the frame + * size by the number of video frames per second, divide the + * result by the number of USB frames (or micro-frames for + * high-speed devices) per second and add the UVC header size + * (assumed to be 12 bytes long). + */ + bandwidth = frame->wWidth * frame->wHeight / 8 * format->bpp; + bandwidth *= 10000000 / interval + 1; + bandwidth /= 1000; + if (stream->dev->udev->speed == USB_SPEED_HIGH) + bandwidth /= 8; + bandwidth += 12; + + /* The bandwidth estimate is too low for many cameras. Don't use + * maximum packet sizes lower than 1024 bytes to try and work + * around the problem. According to measurements done on two + * different camera models, the value is high enough to get most + * resolutions working while not preventing two simultaneous + * VGA streams at 15 fps. + */ + bandwidth = max_t(u32, bandwidth, 1024); + + ctrl->dwMaxPayloadTransferSize = bandwidth; + } +} + +static int uvc_get_video_ctrl(struct uvc_streaming *stream, + struct uvc_streaming_control *ctrl, int probe, __u8 query) +{ + __u8 *data; + __u16 size; + int ret; + + size = stream->dev->uvc_version >= 0x0110 ? 34 : 26; + if ((stream->dev->quirks & UVC_QUIRK_PROBE_DEF) && + query == UVC_GET_DEF) + return -EIO; + + data = kmalloc(size, GFP_KERNEL); + if (data == NULL) + return -ENOMEM; + + ret = __uvc_query_ctrl(stream->dev, query, 0, stream->intfnum, + probe ? UVC_VS_PROBE_CONTROL : UVC_VS_COMMIT_CONTROL, data, + size, uvc_timeout_param); + + if ((query == UVC_GET_MIN || query == UVC_GET_MAX) && ret == 2) { + /* Some cameras, mostly based on Bison Electronics chipsets, + * answer a GET_MIN or GET_MAX request with the wCompQuality + * field only. + */ + uvc_warn_once(stream->dev, UVC_WARN_MINMAX, "UVC non " + "compliance - GET_MIN/MAX(PROBE) incorrectly " + "supported. Enabling workaround.\n"); + memset(ctrl, 0, sizeof *ctrl); + ctrl->wCompQuality = le16_to_cpup((__le16 *)data); + ret = 0; + goto out; + } else if (query == UVC_GET_DEF && probe == 1 && ret != size) { + /* Many cameras don't support the GET_DEF request on their + * video probe control. Warn once and return, the caller will + * fall back to GET_CUR. + */ + uvc_warn_once(stream->dev, UVC_WARN_PROBE_DEF, "UVC non " + "compliance - GET_DEF(PROBE) not supported. " + "Enabling workaround.\n"); + ret = -EIO; + goto out; + } else if (ret != size) { + uvc_printk(KERN_ERR, "Failed to query (%u) UVC %s control : " + "%d (exp. %u).\n", query, probe ? "probe" : "commit", + ret, size); + ret = -EIO; + goto out; + } + + ctrl->bmHint = le16_to_cpup((__le16 *)&data[0]); + ctrl->bFormatIndex = data[2]; + ctrl->bFrameIndex = data[3]; + ctrl->dwFrameInterval = le32_to_cpup((__le32 *)&data[4]); + ctrl->wKeyFrameRate = le16_to_cpup((__le16 *)&data[8]); + ctrl->wPFrameRate = le16_to_cpup((__le16 *)&data[10]); + ctrl->wCompQuality = le16_to_cpup((__le16 *)&data[12]); + ctrl->wCompWindowSize = le16_to_cpup((__le16 *)&data[14]); + ctrl->wDelay = le16_to_cpup((__le16 *)&data[16]); + ctrl->dwMaxVideoFrameSize = get_unaligned_le32(&data[18]); + ctrl->dwMaxPayloadTransferSize = get_unaligned_le32(&data[22]); + + if (size == 34) { + ctrl->dwClockFrequency = get_unaligned_le32(&data[26]); + ctrl->bmFramingInfo = data[30]; + ctrl->bPreferedVersion = data[31]; + ctrl->bMinVersion = data[32]; + ctrl->bMaxVersion = data[33]; + } else { + ctrl->dwClockFrequency = stream->dev->clock_frequency; + ctrl->bmFramingInfo = 0; + ctrl->bPreferedVersion = 0; + ctrl->bMinVersion = 0; + ctrl->bMaxVersion = 0; + } + + /* Some broken devices return null or wrong dwMaxVideoFrameSize and + * dwMaxPayloadTransferSize fields. Try to get the value from the + * format and frame descriptors. + */ + uvc_fixup_video_ctrl(stream, ctrl); + ret = 0; + +out: + kfree(data); + return ret; +} + +static int uvc_set_video_ctrl(struct uvc_streaming *stream, + struct uvc_streaming_control *ctrl, int probe) +{ + __u8 *data; + __u16 size; + int ret; + + size = stream->dev->uvc_version >= 0x0110 ? 34 : 26; + data = kzalloc(size, GFP_KERNEL); + if (data == NULL) + return -ENOMEM; + + *(__le16 *)&data[0] = cpu_to_le16(ctrl->bmHint); + data[2] = ctrl->bFormatIndex; + data[3] = ctrl->bFrameIndex; + *(__le32 *)&data[4] = cpu_to_le32(ctrl->dwFrameInterval); + *(__le16 *)&data[8] = cpu_to_le16(ctrl->wKeyFrameRate); + *(__le16 *)&data[10] = cpu_to_le16(ctrl->wPFrameRate); + *(__le16 *)&data[12] = cpu_to_le16(ctrl->wCompQuality); + *(__le16 *)&data[14] = cpu_to_le16(ctrl->wCompWindowSize); + *(__le16 *)&data[16] = cpu_to_le16(ctrl->wDelay); + put_unaligned_le32(ctrl->dwMaxVideoFrameSize, &data[18]); + put_unaligned_le32(ctrl->dwMaxPayloadTransferSize, &data[22]); + + if (size == 34) { + put_unaligned_le32(ctrl->dwClockFrequency, &data[26]); + data[30] = ctrl->bmFramingInfo; + data[31] = ctrl->bPreferedVersion; + data[32] = ctrl->bMinVersion; + data[33] = ctrl->bMaxVersion; + } + + ret = __uvc_query_ctrl(stream->dev, UVC_SET_CUR, 0, stream->intfnum, + probe ? UVC_VS_PROBE_CONTROL : UVC_VS_COMMIT_CONTROL, data, + size, uvc_timeout_param); + if (ret != size) { + uvc_printk(KERN_ERR, "Failed to set UVC %s control : " + "%d (exp. %u).\n", probe ? "probe" : "commit", + ret, size); + ret = -EIO; + } + + kfree(data); + return ret; +} + +int uvc_probe_video(struct uvc_streaming *stream, + struct uvc_streaming_control *probe) +{ + struct uvc_streaming_control probe_min, probe_max; + __u16 bandwidth; + unsigned int i; + int ret; + + /* Perform probing. The device should adjust the requested values + * according to its capabilities. However, some devices, namely the + * first generation UVC Logitech webcams, don't implement the Video + * Probe control properly, and just return the needed bandwidth. For + * that reason, if the needed bandwidth exceeds the maximum available + * bandwidth, try to lower the quality. + */ + ret = uvc_set_video_ctrl(stream, probe, 1); + if (ret < 0) + goto done; + + /* Get the minimum and maximum values for compression settings. */ + if (!(stream->dev->quirks & UVC_QUIRK_PROBE_MINMAX)) { + ret = uvc_get_video_ctrl(stream, &probe_min, 1, UVC_GET_MIN); + if (ret < 0) + goto done; + ret = uvc_get_video_ctrl(stream, &probe_max, 1, UVC_GET_MAX); + if (ret < 0) + goto done; + + probe->wCompQuality = probe_max.wCompQuality; + } + + for (i = 0; i < 2; ++i) { + ret = uvc_set_video_ctrl(stream, probe, 1); + if (ret < 0) + goto done; + ret = uvc_get_video_ctrl(stream, probe, 1, UVC_GET_CUR); + if (ret < 0) + goto done; + + if (stream->intf->num_altsetting == 1) + break; + + bandwidth = probe->dwMaxPayloadTransferSize; + if (bandwidth <= stream->maxpsize) + break; + + if (stream->dev->quirks & UVC_QUIRK_PROBE_MINMAX) { + ret = -ENOSPC; + goto done; + } + + /* TODO: negotiate compression parameters */ + probe->wKeyFrameRate = probe_min.wKeyFrameRate; + probe->wPFrameRate = probe_min.wPFrameRate; + probe->wCompQuality = probe_max.wCompQuality; + probe->wCompWindowSize = probe_min.wCompWindowSize; + } + +done: + return ret; +} + +static int uvc_commit_video(struct uvc_streaming *stream, + struct uvc_streaming_control *probe) +{ + return uvc_set_video_ctrl(stream, probe, 0); +} + +/* ----------------------------------------------------------------------------- + * Clocks and timestamps + */ + +static void +uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf, + const __u8 *data, int len) +{ + struct uvc_clock_sample *sample; + unsigned int header_size; + bool has_pts = false; + bool has_scr = false; + unsigned long flags; + struct timespec ts; + u16 host_sof; + u16 dev_sof; + + switch (data[1] & (UVC_STREAM_PTS | UVC_STREAM_SCR)) { + case UVC_STREAM_PTS | UVC_STREAM_SCR: + header_size = 12; + has_pts = true; + has_scr = true; + break; + case UVC_STREAM_PTS: + header_size = 6; + has_pts = true; + break; + case UVC_STREAM_SCR: + header_size = 8; + has_scr = true; + break; + default: + header_size = 2; + break; + } + + /* Check for invalid headers. */ + if (len < header_size) + return; + + /* Extract the timestamps: + * + * - store the frame PTS in the buffer structure + * - if the SCR field is present, retrieve the host SOF counter and + * kernel timestamps and store them with the SCR STC and SOF fields + * in the ring buffer + */ + if (has_pts && buf != NULL) + buf->pts = get_unaligned_le32(&data[2]); + + if (!has_scr) + return; + + /* To limit the amount of data, drop SCRs with an SOF identical to the + * previous one. + */ + dev_sof = get_unaligned_le16(&data[header_size - 2]); + if (dev_sof == stream->clock.last_sof) + return; + + stream->clock.last_sof = dev_sof; + + host_sof = usb_get_current_frame_number(stream->dev->udev); + ktime_get_ts(&ts); + + /* The UVC specification allows device implementations that can't obtain + * the USB frame number to keep their own frame counters as long as they + * match the size and frequency of the frame number associated with USB + * SOF tokens. The SOF values sent by such devices differ from the USB + * SOF tokens by a fixed offset that needs to be estimated and accounted + * for to make timestamp recovery as accurate as possible. + * + * The offset is estimated the first time a device SOF value is received + * as the difference between the host and device SOF values. As the two + * SOF values can differ slightly due to transmission delays, consider + * that the offset is null if the difference is not higher than 10 ms + * (negative differences can not happen and are thus considered as an + * offset). The video commit control wDelay field should be used to + * compute a dynamic threshold instead of using a fixed 10 ms value, but + * devices don't report reliable wDelay values. + * + * See uvc_video_clock_host_sof() for an explanation regarding why only + * the 8 LSBs of the delta are kept. + */ + if (stream->clock.sof_offset == (u16)-1) { + u16 delta_sof = (host_sof - dev_sof) & 255; + if (delta_sof >= 10) + stream->clock.sof_offset = delta_sof; + else + stream->clock.sof_offset = 0; + } + + dev_sof = (dev_sof + stream->clock.sof_offset) & 2047; + + spin_lock_irqsave(&stream->clock.lock, flags); + + sample = &stream->clock.samples[stream->clock.head]; + sample->dev_stc = get_unaligned_le32(&data[header_size - 6]); + sample->dev_sof = dev_sof; + sample->host_sof = host_sof; + sample->host_ts = ts; + + /* Update the sliding window head and count. */ + stream->clock.head = (stream->clock.head + 1) % stream->clock.size; + + if (stream->clock.count < stream->clock.size) + stream->clock.count++; + + spin_unlock_irqrestore(&stream->clock.lock, flags); +} + +static void uvc_video_clock_reset(struct uvc_streaming *stream) +{ + struct uvc_clock *clock = &stream->clock; + + clock->head = 0; + clock->count = 0; + clock->last_sof = -1; + clock->sof_offset = -1; +} + +static int uvc_video_clock_init(struct uvc_streaming *stream) +{ + struct uvc_clock *clock = &stream->clock; + + spin_lock_init(&clock->lock); + clock->size = 32; + + clock->samples = kmalloc(clock->size * sizeof(*clock->samples), + GFP_KERNEL); + if (clock->samples == NULL) + return -ENOMEM; + + uvc_video_clock_reset(stream); + + return 0; +} + +static void uvc_video_clock_cleanup(struct uvc_streaming *stream) +{ + kfree(stream->clock.samples); + stream->clock.samples = NULL; +} + +/* + * uvc_video_clock_host_sof - Return the host SOF value for a clock sample + * + * Host SOF counters reported by usb_get_current_frame_number() usually don't + * cover the whole 11-bits SOF range (0-2047) but are limited to the HCI frame + * schedule window. They can be limited to 8, 9 or 10 bits depending on the host + * controller and its configuration. + * + * We thus need to recover the SOF value corresponding to the host frame number. + * As the device and host frame numbers are sampled in a short interval, the + * difference between their values should be equal to a small delta plus an + * integer multiple of 256 caused by the host frame number limited precision. + * + * To obtain the recovered host SOF value, compute the small delta by masking + * the high bits of the host frame counter and device SOF difference and add it + * to the device SOF value. + */ +static u16 uvc_video_clock_host_sof(const struct uvc_clock_sample *sample) +{ + /* The delta value can be negative. */ + s8 delta_sof; + + delta_sof = (sample->host_sof - sample->dev_sof) & 255; + + return (sample->dev_sof + delta_sof) & 2047; +} + +/* + * uvc_video_clock_update - Update the buffer timestamp + * + * This function converts the buffer PTS timestamp to the host clock domain by + * going through the USB SOF clock domain and stores the result in the V4L2 + * buffer timestamp field. + * + * The relationship between the device clock and the host clock isn't known. + * However, the device and the host share the common USB SOF clock which can be + * used to recover that relationship. + * + * The relationship between the device clock and the USB SOF clock is considered + * to be linear over the clock samples sliding window and is given by + * + * SOF = m * PTS + p + * + * Several methods to compute the slope (m) and intercept (p) can be used. As + * the clock drift should be small compared to the sliding window size, we + * assume that the line that goes through the points at both ends of the window + * is a good approximation. Naming those points P1 and P2, we get + * + * SOF = (SOF2 - SOF1) / (STC2 - STC1) * PTS + * + (SOF1 * STC2 - SOF2 * STC1) / (STC2 - STC1) + * + * or + * + * SOF = ((SOF2 - SOF1) * PTS + SOF1 * STC2 - SOF2 * STC1) / (STC2 - STC1) (1) + * + * to avoid loosing precision in the division. Similarly, the host timestamp is + * computed with + * + * TS = ((TS2 - TS1) * PTS + TS1 * SOF2 - TS2 * SOF1) / (SOF2 - SOF1) (2) + * + * SOF values are coded on 11 bits by USB. We extend their precision with 16 + * decimal bits, leading to a 11.16 coding. + * + * TODO: To avoid surprises with device clock values, PTS/STC timestamps should + * be normalized using the nominal device clock frequency reported through the + * UVC descriptors. + * + * Both the PTS/STC and SOF counters roll over, after a fixed but device + * specific amount of time for PTS/STC and after 2048ms for SOF. As long as the + * sliding window size is smaller than the rollover period, differences computed + * on unsigned integers will produce the correct result. However, the p term in + * the linear relations will be miscomputed. + * + * To fix the issue, we subtract a constant from the PTS and STC values to bring + * PTS to half the 32 bit STC range. The sliding window STC values then fit into + * the 32 bit range without any rollover. + * + * Similarly, we add 2048 to the device SOF values to make sure that the SOF + * computed by (1) will never be smaller than 0. This offset is then compensated + * by adding 2048 to the SOF values used in (2). However, this doesn't prevent + * rollovers between (1) and (2): the SOF value computed by (1) can be slightly + * lower than 4096, and the host SOF counters can have rolled over to 2048. This + * case is handled by subtracting 2048 from the SOF value if it exceeds the host + * SOF value at the end of the sliding window. + * + * Finally we subtract a constant from the host timestamps to bring the first + * timestamp of the sliding window to 1s. + */ +void uvc_video_clock_update(struct uvc_streaming *stream, + struct v4l2_buffer *v4l2_buf, + struct uvc_buffer *buf) +{ + struct uvc_clock *clock = &stream->clock; + struct uvc_clock_sample *first; + struct uvc_clock_sample *last; + unsigned long flags; + struct timespec ts; + u32 delta_stc; + u32 y1, y2; + u32 x1, x2; + u32 mean; + u32 sof; + u32 div; + u32 rem; + u64 y; + + spin_lock_irqsave(&clock->lock, flags); + + if (clock->count < clock->size) + goto done; + + first = &clock->samples[clock->head]; + last = &clock->samples[(clock->head - 1) % clock->size]; + + /* First step, PTS to SOF conversion. */ + delta_stc = buf->pts - (1UL << 31); + x1 = first->dev_stc - delta_stc; + x2 = last->dev_stc - delta_stc; + if (x1 == x2) + goto done; + + y1 = (first->dev_sof + 2048) << 16; + y2 = (last->dev_sof + 2048) << 16; + if (y2 < y1) + y2 += 2048 << 16; + + y = (u64)(y2 - y1) * (1ULL << 31) + (u64)y1 * (u64)x2 + - (u64)y2 * (u64)x1; + y = div_u64(y, x2 - x1); + + sof = y; + + uvc_trace(UVC_TRACE_CLOCK, "%s: PTS %u y %llu.%06llu SOF %u.%06llu " + "(x1 %u x2 %u y1 %u y2 %u SOF offset %u)\n", + stream->dev->name, buf->pts, + y >> 16, div_u64((y & 0xffff) * 1000000, 65536), + sof >> 16, div_u64(((u64)sof & 0xffff) * 1000000LLU, 65536), + x1, x2, y1, y2, clock->sof_offset); + + /* Second step, SOF to host clock conversion. */ + x1 = (uvc_video_clock_host_sof(first) + 2048) << 16; + x2 = (uvc_video_clock_host_sof(last) + 2048) << 16; + if (x2 < x1) + x2 += 2048 << 16; + if (x1 == x2) + goto done; + + ts = timespec_sub(last->host_ts, first->host_ts); + y1 = NSEC_PER_SEC; + y2 = (ts.tv_sec + 1) * NSEC_PER_SEC + ts.tv_nsec; + + /* Interpolated and host SOF timestamps can wrap around at slightly + * different times. Handle this by adding or removing 2048 to or from + * the computed SOF value to keep it close to the SOF samples mean + * value. + */ + mean = (x1 + x2) / 2; + if (mean - (1024 << 16) > sof) + sof += 2048 << 16; + else if (sof > mean + (1024 << 16)) + sof -= 2048 << 16; + + y = (u64)(y2 - y1) * (u64)sof + (u64)y1 * (u64)x2 + - (u64)y2 * (u64)x1; + y = div_u64(y, x2 - x1); + + div = div_u64_rem(y, NSEC_PER_SEC, &rem); + ts.tv_sec = first->host_ts.tv_sec - 1 + div; + ts.tv_nsec = first->host_ts.tv_nsec + rem; + if (ts.tv_nsec >= NSEC_PER_SEC) { + ts.tv_sec++; + ts.tv_nsec -= NSEC_PER_SEC; + } + + uvc_trace(UVC_TRACE_CLOCK, "%s: SOF %u.%06llu y %llu ts %lu.%06lu " + "buf ts %lu.%06lu (x1 %u/%u/%u x2 %u/%u/%u y1 %u y2 %u)\n", + stream->dev->name, + sof >> 16, div_u64(((u64)sof & 0xffff) * 1000000LLU, 65536), + y, ts.tv_sec, ts.tv_nsec / NSEC_PER_USEC, + v4l2_buf->timestamp.tv_sec, v4l2_buf->timestamp.tv_usec, + x1, first->host_sof, first->dev_sof, + x2, last->host_sof, last->dev_sof, y1, y2); + + /* Update the V4L2 buffer. */ + v4l2_buf->timestamp.tv_sec = ts.tv_sec; + v4l2_buf->timestamp.tv_usec = ts.tv_nsec / NSEC_PER_USEC; + +done: + spin_unlock_irqrestore(&stream->clock.lock, flags); +} + +/* ------------------------------------------------------------------------ + * Stream statistics + */ + +static void uvc_video_stats_decode(struct uvc_streaming *stream, + const __u8 *data, int len) +{ + unsigned int header_size; + bool has_pts = false; + bool has_scr = false; + u16 uninitialized_var(scr_sof); + u32 uninitialized_var(scr_stc); + u32 uninitialized_var(pts); + + if (stream->stats.stream.nb_frames == 0 && + stream->stats.frame.nb_packets == 0) + ktime_get_ts(&stream->stats.stream.start_ts); + + switch (data[1] & (UVC_STREAM_PTS | UVC_STREAM_SCR)) { + case UVC_STREAM_PTS | UVC_STREAM_SCR: + header_size = 12; + has_pts = true; + has_scr = true; + break; + case UVC_STREAM_PTS: + header_size = 6; + has_pts = true; + break; + case UVC_STREAM_SCR: + header_size = 8; + has_scr = true; + break; + default: + header_size = 2; + break; + } + + /* Check for invalid headers. */ + if (len < header_size || data[0] < header_size) { + stream->stats.frame.nb_invalid++; + return; + } + + /* Extract the timestamps. */ + if (has_pts) + pts = get_unaligned_le32(&data[2]); + + if (has_scr) { + scr_stc = get_unaligned_le32(&data[header_size - 6]); + scr_sof = get_unaligned_le16(&data[header_size - 2]); + } + + /* Is PTS constant through the whole frame ? */ + if (has_pts && stream->stats.frame.nb_pts) { + if (stream->stats.frame.pts != pts) { + stream->stats.frame.nb_pts_diffs++; + stream->stats.frame.last_pts_diff = + stream->stats.frame.nb_packets; + } + } + + if (has_pts) { + stream->stats.frame.nb_pts++; + stream->stats.frame.pts = pts; + } + + /* Do all frames have a PTS in their first non-empty packet, or before + * their first empty packet ? + */ + if (stream->stats.frame.size == 0) { + if (len > header_size) + stream->stats.frame.has_initial_pts = has_pts; + if (len == header_size && has_pts) + stream->stats.frame.has_early_pts = true; + } + + /* Do the SCR.STC and SCR.SOF fields vary through the frame ? */ + if (has_scr && stream->stats.frame.nb_scr) { + if (stream->stats.frame.scr_stc != scr_stc) + stream->stats.frame.nb_scr_diffs++; + } + + if (has_scr) { + /* Expand the SOF counter to 32 bits and store its value. */ + if (stream->stats.stream.nb_frames > 0 || + stream->stats.frame.nb_scr > 0) + stream->stats.stream.scr_sof_count += + (scr_sof - stream->stats.stream.scr_sof) % 2048; + stream->stats.stream.scr_sof = scr_sof; + + stream->stats.frame.nb_scr++; + stream->stats.frame.scr_stc = scr_stc; + stream->stats.frame.scr_sof = scr_sof; + + if (scr_sof < stream->stats.stream.min_sof) + stream->stats.stream.min_sof = scr_sof; + if (scr_sof > stream->stats.stream.max_sof) + stream->stats.stream.max_sof = scr_sof; + } + + /* Record the first non-empty packet number. */ + if (stream->stats.frame.size == 0 && len > header_size) + stream->stats.frame.first_data = stream->stats.frame.nb_packets; + + /* Update the frame size. */ + stream->stats.frame.size += len - header_size; + + /* Update the packets counters. */ + stream->stats.frame.nb_packets++; + if (len > header_size) + stream->stats.frame.nb_empty++; + + if (data[1] & UVC_STREAM_ERR) + stream->stats.frame.nb_errors++; +} + +static void uvc_video_stats_update(struct uvc_streaming *stream) +{ + struct uvc_stats_frame *frame = &stream->stats.frame; + + uvc_trace(UVC_TRACE_STATS, "frame %u stats: %u/%u/%u packets, " + "%u/%u/%u pts (%searly %sinitial), %u/%u scr, " + "last pts/stc/sof %u/%u/%u\n", + stream->sequence, frame->first_data, + frame->nb_packets - frame->nb_empty, frame->nb_packets, + frame->nb_pts_diffs, frame->last_pts_diff, frame->nb_pts, + frame->has_early_pts ? "" : "!", + frame->has_initial_pts ? "" : "!", + frame->nb_scr_diffs, frame->nb_scr, + frame->pts, frame->scr_stc, frame->scr_sof); + + stream->stats.stream.nb_frames++; + stream->stats.stream.nb_packets += stream->stats.frame.nb_packets; + stream->stats.stream.nb_empty += stream->stats.frame.nb_empty; + stream->stats.stream.nb_errors += stream->stats.frame.nb_errors; + stream->stats.stream.nb_invalid += stream->stats.frame.nb_invalid; + + if (frame->has_early_pts) + stream->stats.stream.nb_pts_early++; + if (frame->has_initial_pts) + stream->stats.stream.nb_pts_initial++; + if (frame->last_pts_diff <= frame->first_data) + stream->stats.stream.nb_pts_constant++; + if (frame->nb_scr >= frame->nb_packets - frame->nb_empty) + stream->stats.stream.nb_scr_count_ok++; + if (frame->nb_scr_diffs + 1 == frame->nb_scr) + stream->stats.stream.nb_scr_diffs_ok++; + + memset(&stream->stats.frame, 0, sizeof(stream->stats.frame)); +} + +size_t uvc_video_stats_dump(struct uvc_streaming *stream, char *buf, + size_t size) +{ + unsigned int scr_sof_freq; + unsigned int duration; + struct timespec ts; + size_t count = 0; + + ts.tv_sec = stream->stats.stream.stop_ts.tv_sec + - stream->stats.stream.start_ts.tv_sec; + ts.tv_nsec = stream->stats.stream.stop_ts.tv_nsec + - stream->stats.stream.start_ts.tv_nsec; + if (ts.tv_nsec < 0) { + ts.tv_sec--; + ts.tv_nsec += 1000000000; + } + + /* Compute the SCR.SOF frequency estimate. At the nominal 1kHz SOF + * frequency this will not overflow before more than 1h. + */ + duration = ts.tv_sec * 1000 + ts.tv_nsec / 1000000; + if (duration != 0) + scr_sof_freq = stream->stats.stream.scr_sof_count * 1000 + / duration; + else + scr_sof_freq = 0; + + count += scnprintf(buf + count, size - count, + "frames: %u\npackets: %u\nempty: %u\n" + "errors: %u\ninvalid: %u\n", + stream->stats.stream.nb_frames, + stream->stats.stream.nb_packets, + stream->stats.stream.nb_empty, + stream->stats.stream.nb_errors, + stream->stats.stream.nb_invalid); + count += scnprintf(buf + count, size - count, + "pts: %u early, %u initial, %u ok\n", + stream->stats.stream.nb_pts_early, + stream->stats.stream.nb_pts_initial, + stream->stats.stream.nb_pts_constant); + count += scnprintf(buf + count, size - count, + "scr: %u count ok, %u diff ok\n", + stream->stats.stream.nb_scr_count_ok, + stream->stats.stream.nb_scr_diffs_ok); + count += scnprintf(buf + count, size - count, + "sof: %u <= sof <= %u, freq %u.%03u kHz\n", + stream->stats.stream.min_sof, + stream->stats.stream.max_sof, + scr_sof_freq / 1000, scr_sof_freq % 1000); + + return count; +} + +static void uvc_video_stats_start(struct uvc_streaming *stream) +{ + memset(&stream->stats, 0, sizeof(stream->stats)); + stream->stats.stream.min_sof = 2048; +} + +static void uvc_video_stats_stop(struct uvc_streaming *stream) +{ + ktime_get_ts(&stream->stats.stream.stop_ts); +} + +/* ------------------------------------------------------------------------ + * Video codecs + */ + +/* Video payload decoding is handled by uvc_video_decode_start(), + * uvc_video_decode_data() and uvc_video_decode_end(). + * + * uvc_video_decode_start is called with URB data at the start of a bulk or + * isochronous payload. It processes header data and returns the header size + * in bytes if successful. If an error occurs, it returns a negative error + * code. The following error codes have special meanings. + * + * - EAGAIN informs the caller that the current video buffer should be marked + * as done, and that the function should be called again with the same data + * and a new video buffer. This is used when end of frame conditions can be + * reliably detected at the beginning of the next frame only. + * + * If an error other than -EAGAIN is returned, the caller will drop the current + * payload. No call to uvc_video_decode_data and uvc_video_decode_end will be + * made until the next payload. -ENODATA can be used to drop the current + * payload if no other error code is appropriate. + * + * uvc_video_decode_data is called for every URB with URB data. It copies the + * data to the video buffer. + * + * uvc_video_decode_end is called with header data at the end of a bulk or + * isochronous payload. It performs any additional header data processing and + * returns 0 or a negative error code if an error occurred. As header data have + * already been processed by uvc_video_decode_start, this functions isn't + * required to perform sanity checks a second time. + * + * For isochronous transfers where a payload is always transferred in a single + * URB, the three functions will be called in a row. + * + * To let the decoder process header data and update its internal state even + * when no video buffer is available, uvc_video_decode_start must be prepared + * to be called with a NULL buf parameter. uvc_video_decode_data and + * uvc_video_decode_end will never be called with a NULL buffer. + */ +static int uvc_video_decode_start(struct uvc_streaming *stream, + struct uvc_buffer *buf, const __u8 *data, int len) +{ + __u8 fid; + + /* Sanity checks: + * - packet must be at least 2 bytes long + * - bHeaderLength value must be at least 2 bytes (see above) + * - bHeaderLength value can't be larger than the packet size. + */ + if (len < 2 || data[0] < 2 || data[0] > len) { + stream->stats.frame.nb_invalid++; + return -EINVAL; + } + + fid = data[1] & UVC_STREAM_FID; + + /* Increase the sequence number regardless of any buffer states, so + * that discontinuous sequence numbers always indicate lost frames. + */ + if (stream->last_fid != fid) { + stream->sequence++; + if (stream->sequence) + uvc_video_stats_update(stream); + } + + uvc_video_clock_decode(stream, buf, data, len); + uvc_video_stats_decode(stream, data, len); + + /* Store the payload FID bit and return immediately when the buffer is + * NULL. + */ + if (buf == NULL) { + stream->last_fid = fid; + return -ENODATA; + } + + /* Mark the buffer as bad if the error bit is set. */ + if (data[1] & UVC_STREAM_ERR) { + uvc_trace(UVC_TRACE_FRAME, "Marking buffer as bad (error bit " + "set).\n"); + buf->error = 1; + } + + /* Synchronize to the input stream by waiting for the FID bit to be + * toggled when the the buffer state is not UVC_BUF_STATE_ACTIVE. + * stream->last_fid is initialized to -1, so the first isochronous + * frame will always be in sync. + * + * If the device doesn't toggle the FID bit, invert stream->last_fid + * when the EOF bit is set to force synchronisation on the next packet. + */ + if (buf->state != UVC_BUF_STATE_ACTIVE) { + struct timespec ts; + + if (fid == stream->last_fid) { + uvc_trace(UVC_TRACE_FRAME, "Dropping payload (out of " + "sync).\n"); + if ((stream->dev->quirks & UVC_QUIRK_STREAM_NO_FID) && + (data[1] & UVC_STREAM_EOF)) + stream->last_fid ^= UVC_STREAM_FID; + return -ENODATA; + } + + if (uvc_clock_param == CLOCK_MONOTONIC) + ktime_get_ts(&ts); + else + ktime_get_real_ts(&ts); + + buf->buf.v4l2_buf.sequence = stream->sequence; + buf->buf.v4l2_buf.timestamp.tv_sec = ts.tv_sec; + buf->buf.v4l2_buf.timestamp.tv_usec = + ts.tv_nsec / NSEC_PER_USEC; + + /* TODO: Handle PTS and SCR. */ + buf->state = UVC_BUF_STATE_ACTIVE; + } + + /* Mark the buffer as done if we're at the beginning of a new frame. + * End of frame detection is better implemented by checking the EOF + * bit (FID bit toggling is delayed by one frame compared to the EOF + * bit), but some devices don't set the bit at end of frame (and the + * last payload can be lost anyway). We thus must check if the FID has + * been toggled. + * + * stream->last_fid is initialized to -1, so the first isochronous + * frame will never trigger an end of frame detection. + * + * Empty buffers (bytesused == 0) don't trigger end of frame detection + * as it doesn't make sense to return an empty buffer. This also + * avoids detecting end of frame conditions at FID toggling if the + * previous payload had the EOF bit set. + */ + if (fid != stream->last_fid && buf->bytesused != 0) { + uvc_trace(UVC_TRACE_FRAME, "Frame complete (FID bit " + "toggled).\n"); + buf->state = UVC_BUF_STATE_READY; + return -EAGAIN; + } + + stream->last_fid = fid; + + return data[0]; +} + +static void uvc_video_decode_data(struct uvc_streaming *stream, + struct uvc_buffer *buf, const __u8 *data, int len) +{ + unsigned int maxlen, nbytes; + void *mem; + + if (len <= 0) + return; + + /* Copy the video data to the buffer. */ + maxlen = buf->length - buf->bytesused; + mem = buf->mem + buf->bytesused; + nbytes = min((unsigned int)len, maxlen); + memcpy(mem, data, nbytes); + buf->bytesused += nbytes; + + /* Complete the current frame if the buffer size was exceeded. */ + if (len > maxlen) { + uvc_trace(UVC_TRACE_FRAME, "Frame co |