1
2
3
4
5
6
7
8
9#include <linux/dma-mapping.h>
10#include <linux/highmem.h>
11#include <linux/kernel.h>
12#include <linux/list.h>
13#include <linux/module.h>
14#include <linux/slab.h>
15#include <linux/usb.h>
16#include <linux/usb/hcd.h>
17#include <linux/videodev2.h>
18#include <linux/vmalloc.h>
19#include <linux/wait.h>
20#include <linux/atomic.h>
21#include <asm/unaligned.h>
22
23#include <media/v4l2-common.h>
24
25#include "uvcvideo.h"
26
27
28
29
30
31static int __uvc_query_ctrl(struct uvc_device *dev, u8 query, u8 unit,
32 u8 intfnum, u8 cs, void *data, u16 size,
33 int timeout)
34{
35 u8 type = USB_TYPE_CLASS | USB_RECIP_INTERFACE;
36 unsigned int pipe;
37
38 pipe = (query & 0x80) ? usb_rcvctrlpipe(dev->udev, 0)
39 : usb_sndctrlpipe(dev->udev, 0);
40 type |= (query & 0x80) ? USB_DIR_IN : USB_DIR_OUT;
41
42 return usb_control_msg(dev->udev, pipe, query, type, cs << 8,
43 unit << 8 | intfnum, data, size, timeout);
44}
45
46static const char *uvc_query_name(u8 query)
47{
48 switch (query) {
49 case UVC_SET_CUR:
50 return "SET_CUR";
51 case UVC_GET_CUR:
52 return "GET_CUR";
53 case UVC_GET_MIN:
54 return "GET_MIN";
55 case UVC_GET_MAX:
56 return "GET_MAX";
57 case UVC_GET_RES:
58 return "GET_RES";
59 case UVC_GET_LEN:
60 return "GET_LEN";
61 case UVC_GET_INFO:
62 return "GET_INFO";
63 case UVC_GET_DEF:
64 return "GET_DEF";
65 default:
66 return "<invalid>";
67 }
68}
69
70int uvc_query_ctrl(struct uvc_device *dev, u8 query, u8 unit,
71 u8 intfnum, u8 cs, void *data, u16 size)
72{
73 int ret;
74 u8 error;
75 u8 tmp;
76
77 ret = __uvc_query_ctrl(dev, query, unit, intfnum, cs, data, size,
78 UVC_CTRL_CONTROL_TIMEOUT);
79 if (likely(ret == size))
80 return 0;
81
82 dev_err(&dev->udev->dev,
83 "Failed to query (%s) UVC control %u on unit %u: %d (exp. %u).\n",
84 uvc_query_name(query), cs, unit, ret, size);
85
86 if (ret != -EPIPE)
87 return ret;
88
89 tmp = *(u8 *)data;
90
91 ret = __uvc_query_ctrl(dev, UVC_GET_CUR, 0, intfnum,
92 UVC_VC_REQUEST_ERROR_CODE_CONTROL, data, 1,
93 UVC_CTRL_CONTROL_TIMEOUT);
94
95 error = *(u8 *)data;
96 *(u8 *)data = tmp;
97
98 if (ret != 1)
99 return ret < 0 ? ret : -EPIPE;
100
101 uvc_dbg(dev, CONTROL, "Control error %u\n", error);
102
103 switch (error) {
104 case 0:
105
106 return -EPIPE;
107 case 1:
108 return -EBUSY;
109 case 2:
110 return -EILSEQ;
111 case 3:
112 return -EREMOTE;
113 case 4:
114 return -ERANGE;
115 case 5:
116 case 6:
117 case 7:
118 case 8:
119 return -EINVAL;
120 default:
121 break;
122 }
123
124 return -EPIPE;
125}
126
127static void uvc_fixup_video_ctrl(struct uvc_streaming *stream,
128 struct uvc_streaming_control *ctrl)
129{
130 static const struct usb_device_id elgato_cam_link_4k = {
131 USB_DEVICE(0x0fd9, 0x0066)
132 };
133 struct uvc_format *format = NULL;
134 struct uvc_frame *frame = NULL;
135 unsigned int i;
136
137
138
139
140
141
142
143
144
145
146
147
148
149 if (usb_match_one_id(stream->dev->intf, &elgato_cam_link_4k) &&
150 ctrl->bmHint > 255) {
151 u8 corrected_format_index = ctrl->bmHint >> 8;
152
153 uvc_dbg(stream->dev, VIDEO,
154 "Correct USB video probe response from {bmHint: 0x%04x, bFormatIndex: %u} to {bmHint: 0x%04x, bFormatIndex: %u}\n",
155 ctrl->bmHint, ctrl->bFormatIndex,
156 1, corrected_format_index);
157 ctrl->bmHint = 1;
158 ctrl->bFormatIndex = corrected_format_index;
159 }
160
161 for (i = 0; i < stream->nformats; ++i) {
162 if (stream->format[i].index == ctrl->bFormatIndex) {
163 format = &stream->format[i];
164 break;
165 }
166 }
167
168 if (format == NULL)
169 return;
170
171 for (i = 0; i < format->nframes; ++i) {
172 if (format->frame[i].bFrameIndex == ctrl->bFrameIndex) {
173 frame = &format->frame[i];
174 break;
175 }
176 }
177
178 if (frame == NULL)
179 return;
180
181 if (!(format->flags & UVC_FMT_FLAG_COMPRESSED) ||
182 (ctrl->dwMaxVideoFrameSize == 0 &&
183 stream->dev->uvc_version < 0x0110))
184 ctrl->dwMaxVideoFrameSize =
185 frame->dwMaxVideoFrameBufferSize;
186
187
188
189
190
191
192 if ((ctrl->dwMaxPayloadTransferSize & 0xffff0000) == 0xffff0000)
193 ctrl->dwMaxPayloadTransferSize &= ~0xffff0000;
194
195 if (!(format->flags & UVC_FMT_FLAG_COMPRESSED) &&
196 stream->dev->quirks & UVC_QUIRK_FIX_BANDWIDTH &&
197 stream->intf->num_altsetting > 1) {
198 u32 interval;
199 u32 bandwidth;
200
201 interval = (ctrl->dwFrameInterval > 100000)
202 ? ctrl->dwFrameInterval
203 : frame->dwFrameInterval[0];
204
205
206
207
208
209
210
211 bandwidth = frame->wWidth * frame->wHeight / 8 * format->bpp;
212 bandwidth *= 10000000 / interval + 1;
213 bandwidth /= 1000;
214 if (stream->dev->udev->speed == USB_SPEED_HIGH)
215 bandwidth /= 8;
216 bandwidth += 12;
217
218
219
220
221
222
223
224
225 bandwidth = max_t(u32, bandwidth, 1024);
226
227 ctrl->dwMaxPayloadTransferSize = bandwidth;
228 }
229}
230
231static size_t uvc_video_ctrl_size(struct uvc_streaming *stream)
232{
233
234
235
236
237 if (stream->dev->uvc_version < 0x0110)
238 return 26;
239 else if (stream->dev->uvc_version < 0x0150)
240 return 34;
241 else
242 return 48;
243}
244
245static int uvc_get_video_ctrl(struct uvc_streaming *stream,
246 struct uvc_streaming_control *ctrl, int probe, u8 query)
247{
248 u16 size = uvc_video_ctrl_size(stream);
249 u8 *data;
250 int ret;
251
252 if ((stream->dev->quirks & UVC_QUIRK_PROBE_DEF) &&
253 query == UVC_GET_DEF)
254 return -EIO;
255
256 data = kmalloc(size, GFP_KERNEL);
257 if (data == NULL)
258 return -ENOMEM;
259
260 ret = __uvc_query_ctrl(stream->dev, query, 0, stream->intfnum,
261 probe ? UVC_VS_PROBE_CONTROL : UVC_VS_COMMIT_CONTROL, data,
262 size, uvc_timeout_param);
263
264 if ((query == UVC_GET_MIN || query == UVC_GET_MAX) && ret == 2) {
265
266
267
268
269 uvc_warn_once(stream->dev, UVC_WARN_MINMAX, "UVC non "
270 "compliance - GET_MIN/MAX(PROBE) incorrectly "
271 "supported. Enabling workaround.\n");
272 memset(ctrl, 0, sizeof(*ctrl));
273 ctrl->wCompQuality = le16_to_cpup((__le16 *)data);
274 ret = 0;
275 goto out;
276 } else if (query == UVC_GET_DEF && probe == 1 && ret != size) {
277
278
279
280
281 uvc_warn_once(stream->dev, UVC_WARN_PROBE_DEF, "UVC non "
282 "compliance - GET_DEF(PROBE) not supported. "
283 "Enabling workaround.\n");
284 ret = -EIO;
285 goto out;
286 } else if (ret != size) {
287 dev_err(&stream->intf->dev,
288 "Failed to query (%u) UVC %s control : %d (exp. %u).\n",
289 query, probe ? "probe" : "commit", ret, size);
290 ret = -EIO;
291 goto out;
292 }
293
294 ctrl->bmHint = le16_to_cpup((__le16 *)&data[0]);
295 ctrl->bFormatIndex = data[2];
296 ctrl->bFrameIndex = data[3];
297 ctrl->dwFrameInterval = le32_to_cpup((__le32 *)&data[4]);
298 ctrl->wKeyFrameRate = le16_to_cpup((__le16 *)&data[8]);
299 ctrl->wPFrameRate = le16_to_cpup((__le16 *)&data[10]);
300 ctrl->wCompQuality = le16_to_cpup((__le16 *)&data[12]);
301 ctrl->wCompWindowSize = le16_to_cpup((__le16 *)&data[14]);
302 ctrl->wDelay = le16_to_cpup((__le16 *)&data[16]);
303 ctrl->dwMaxVideoFrameSize = get_unaligned_le32(&data[18]);
304 ctrl->dwMaxPayloadTransferSize = get_unaligned_le32(&data[22]);
305
306 if (size >= 34) {
307 ctrl->dwClockFrequency = get_unaligned_le32(&data[26]);
308 ctrl->bmFramingInfo = data[30];
309 ctrl->bPreferedVersion = data[31];
310 ctrl->bMinVersion = data[32];
311 ctrl->bMaxVersion = data[33];
312 } else {
313 ctrl->dwClockFrequency = stream->dev->clock_frequency;
314 ctrl->bmFramingInfo = 0;
315 ctrl->bPreferedVersion = 0;
316 ctrl->bMinVersion = 0;
317 ctrl->bMaxVersion = 0;
318 }
319
320
321
322
323
324 uvc_fixup_video_ctrl(stream, ctrl);
325 ret = 0;
326
327out:
328 kfree(data);
329 return ret;
330}
331
332static int uvc_set_video_ctrl(struct uvc_streaming *stream,
333 struct uvc_streaming_control *ctrl, int probe)
334{
335 u16 size = uvc_video_ctrl_size(stream);
336 u8 *data;
337 int ret;
338
339 data = kzalloc(size, GFP_KERNEL);
340 if (data == NULL)
341 return -ENOMEM;
342
343 *(__le16 *)&data[0] = cpu_to_le16(ctrl->bmHint);
344 data[2] = ctrl->bFormatIndex;
345 data[3] = ctrl->bFrameIndex;
346 *(__le32 *)&data[4] = cpu_to_le32(ctrl->dwFrameInterval);
347 *(__le16 *)&data[8] = cpu_to_le16(ctrl->wKeyFrameRate);
348 *(__le16 *)&data[10] = cpu_to_le16(ctrl->wPFrameRate);
349 *(__le16 *)&data[12] = cpu_to_le16(ctrl->wCompQuality);
350 *(__le16 *)&data[14] = cpu_to_le16(ctrl->wCompWindowSize);
351 *(__le16 *)&data[16] = cpu_to_le16(ctrl->wDelay);
352 put_unaligned_le32(ctrl->dwMaxVideoFrameSize, &data[18]);
353 put_unaligned_le32(ctrl->dwMaxPayloadTransferSize, &data[22]);
354
355 if (size >= 34) {
356 put_unaligned_le32(ctrl->dwClockFrequency, &data[26]);
357 data[30] = ctrl->bmFramingInfo;
358 data[31] = ctrl->bPreferedVersion;
359 data[32] = ctrl->bMinVersion;
360 data[33] = ctrl->bMaxVersion;
361 }
362
363 ret = __uvc_query_ctrl(stream->dev, UVC_SET_CUR, 0, stream->intfnum,
364 probe ? UVC_VS_PROBE_CONTROL : UVC_VS_COMMIT_CONTROL, data,
365 size, uvc_timeout_param);
366 if (ret != size) {
367 dev_err(&stream->intf->dev,
368 "Failed to set UVC %s control : %d (exp. %u).\n",
369 probe ? "probe" : "commit", ret, size);
370 ret = -EIO;
371 }
372
373 kfree(data);
374 return ret;
375}
376
377int uvc_probe_video(struct uvc_streaming *stream,
378 struct uvc_streaming_control *probe)
379{
380 struct uvc_streaming_control probe_min, probe_max;
381 u16 bandwidth;
382 unsigned int i;
383 int ret;
384
385
386
387
388
389
390
391
392 ret = uvc_set_video_ctrl(stream, probe, 1);
393 if (ret < 0)
394 goto done;
395
396
397 if (!(stream->dev->quirks & UVC_QUIRK_PROBE_MINMAX)) {
398 ret = uvc_get_video_ctrl(stream, &probe_min, 1, UVC_GET_MIN);
399 if (ret < 0)
400 goto done;
401 ret = uvc_get_video_ctrl(stream, &probe_max, 1, UVC_GET_MAX);
402 if (ret < 0)
403 goto done;
404
405 probe->wCompQuality = probe_max.wCompQuality;
406 }
407
408 for (i = 0; i < 2; ++i) {
409 ret = uvc_set_video_ctrl(stream, probe, 1);
410 if (ret < 0)
411 goto done;
412 ret = uvc_get_video_ctrl(stream, probe, 1, UVC_GET_CUR);
413 if (ret < 0)
414 goto done;
415
416 if (stream->intf->num_altsetting == 1)
417 break;
418
419 bandwidth = probe->dwMaxPayloadTransferSize;
420 if (bandwidth <= stream->maxpsize)
421 break;
422
423 if (stream->dev->quirks & UVC_QUIRK_PROBE_MINMAX) {
424 ret = -ENOSPC;
425 goto done;
426 }
427
428
429 probe->wKeyFrameRate = probe_min.wKeyFrameRate;
430 probe->wPFrameRate = probe_min.wPFrameRate;
431 probe->wCompQuality = probe_max.wCompQuality;
432 probe->wCompWindowSize = probe_min.wCompWindowSize;
433 }
434
435done:
436 return ret;
437}
438
439static int uvc_commit_video(struct uvc_streaming *stream,
440 struct uvc_streaming_control *probe)
441{
442 return uvc_set_video_ctrl(stream, probe, 0);
443}
444
445
446
447
448
449static inline ktime_t uvc_video_get_time(void)
450{
451 if (uvc_clock_param == CLOCK_MONOTONIC)
452 return ktime_get();
453 else
454 return ktime_get_real();
455}
456
457static void
458uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf,
459 const u8 *data, int len)
460{
461 struct uvc_clock_sample *sample;
462 unsigned int header_size;
463 bool has_pts = false;
464 bool has_scr = false;
465 unsigned long flags;
466 ktime_t time;
467 u16 host_sof;
468 u16 dev_sof;
469
470 switch (data[1] & (UVC_STREAM_PTS | UVC_STREAM_SCR)) {
471 case UVC_STREAM_PTS | UVC_STREAM_SCR:
472 header_size = 12;
473 has_pts = true;
474 has_scr = true;
475 break;
476 case UVC_STREAM_PTS:
477 header_size = 6;
478 has_pts = true;
479 break;
480 case UVC_STREAM_SCR:
481 header_size = 8;
482 has_scr = true;
483 break;
484 default:
485 header_size = 2;
486 break;
487 }
488
489
490 if (len < header_size)
491 return;
492
493
494
495
496
497
498
499
500 if (has_pts && buf != NULL)
501 buf->pts = get_unaligned_le32(&data[2]);
502
503 if (!has_scr)
504 return;
505
506
507
508
509 dev_sof = get_unaligned_le16(&data[header_size - 2]);
510 if (dev_sof == stream->clock.last_sof)
511 return;
512
513 stream->clock.last_sof = dev_sof;
514
515 host_sof = usb_get_current_frame_number(stream->dev->udev);
516 time = uvc_video_get_time();
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537 if (stream->clock.sof_offset == (u16)-1) {
538 u16 delta_sof = (host_sof - dev_sof) & 255;
539 if (delta_sof >= 10)
540 stream->clock.sof_offset = delta_sof;
541 else
542 stream->clock.sof_offset = 0;
543 }
544
545 dev_sof = (dev_sof + stream->clock.sof_offset) & 2047;
546
547 spin_lock_irqsave(&stream->clock.lock, flags);
548
549 sample = &stream->clock.samples[stream->clock.head];
550 sample->dev_stc = get_unaligned_le32(&data[header_size - 6]);
551 sample->dev_sof = dev_sof;
552 sample->host_sof = host_sof;
553 sample->host_time = time;
554
555
556 stream->clock.head = (stream->clock.head + 1) % stream->clock.size;
557
558 if (stream->clock.count < stream->clock.size)
559 stream->clock.count++;
560
561 spin_unlock_irqrestore(&stream->clock.lock, flags);
562}
563
564static void uvc_video_clock_reset(struct uvc_streaming *stream)
565{
566 struct uvc_clock *clock = &stream->clock;
567
568 clock->head = 0;
569 clock->count = 0;
570 clock->last_sof = -1;
571 clock->sof_offset = -1;
572}
573
574static int uvc_video_clock_init(struct uvc_streaming *stream)
575{
576 struct uvc_clock *clock = &stream->clock;
577
578 spin_lock_init(&clock->lock);
579 clock->size = 32;
580
581 clock->samples = kmalloc_array(clock->size, sizeof(*clock->samples),
582 GFP_KERNEL);
583 if (clock->samples == NULL)
584 return -ENOMEM;
585
586 uvc_video_clock_reset(stream);
587
588 return 0;
589}
590
591static void uvc_video_clock_cleanup(struct uvc_streaming *stream)
592{
593 kfree(stream->clock.samples);
594 stream->clock.samples = NULL;
595}
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614static u16 uvc_video_clock_host_sof(const struct uvc_clock_sample *sample)
615{
616
617 s8 delta_sof;
618
619 delta_sof = (sample->host_sof - sample->dev_sof) & 255;
620
621 return (sample->dev_sof + delta_sof) & 2047;
622}
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685void uvc_video_clock_update(struct uvc_streaming *stream,
686 struct vb2_v4l2_buffer *vbuf,
687 struct uvc_buffer *buf)
688{
689 struct uvc_clock *clock = &stream->clock;
690 struct uvc_clock_sample *first;
691 struct uvc_clock_sample *last;
692 unsigned long flags;
693 u64 timestamp;
694 u32 delta_stc;
695 u32 y1, y2;
696 u32 x1, x2;
697 u32 mean;
698 u32 sof;
699 u64 y;
700
701 if (!uvc_hw_timestamps_param)
702 return;
703
704
705
706
707
708
709 if (!clock->samples)
710 return;
711
712 spin_lock_irqsave(&clock->lock, flags);
713
714 if (clock->count < clock->size)
715 goto done;
716
717 first = &clock->samples[clock->head];
718 last = &clock->samples[(clock->head - 1) % clock->size];
719
720
721 delta_stc = buf->pts - (1UL << 31);
722 x1 = first->dev_stc - delta_stc;
723 x2 = last->dev_stc - delta_stc;
724 if (x1 == x2)
725 goto done;
726
727 y1 = (first->dev_sof + 2048) << 16;
728 y2 = (last->dev_sof + 2048) << 16;
729 if (y2 < y1)
730 y2 += 2048 << 16;
731
732 y = (u64)(y2 - y1) * (1ULL << 31) + (u64)y1 * (u64)x2
733 - (u64)y2 * (u64)x1;
734 y = div_u64(y, x2 - x1);
735
736 sof = y;
737
738 uvc_dbg(stream->dev, CLOCK,
739 "%s: PTS %u y %llu.%06llu SOF %u.%06llu (x1 %u x2 %u y1 %u y2 %u SOF offset %u)\n",
740 stream->dev->name, buf->pts,
741 y >> 16, div_u64((y & 0xffff) * 1000000, 65536),
742 sof >> 16, div_u64(((u64)sof & 0xffff) * 1000000LLU, 65536),
743 x1, x2, y1, y2, clock->sof_offset);
744
745
746 x1 = (uvc_video_clock_host_sof(first) + 2048) << 16;
747 x2 = (uvc_video_clock_host_sof(last) + 2048) << 16;
748 if (x2 < x1)
749 x2 += 2048 << 16;
750 if (x1 == x2)
751 goto done;
752
753 y1 = NSEC_PER_SEC;
754 y2 = (u32)ktime_to_ns(ktime_sub(last->host_time, first->host_time)) + y1;
755
756
757
758
759
760
761 mean = (x1 + x2) / 2;
762 if (mean - (1024 << 16) > sof)
763 sof += 2048 << 16;
764 else if (sof > mean + (1024 << 16))
765 sof -= 2048 << 16;
766
767 y = (u64)(y2 - y1) * (u64)sof + (u64)y1 * (u64)x2
768 - (u64)y2 * (u64)x1;
769 y = div_u64(y, x2 - x1);
770
771 timestamp = ktime_to_ns(first->host_time) + y - y1;
772
773 uvc_dbg(stream->dev, CLOCK,
774 "%s: SOF %u.%06llu y %llu ts %llu buf ts %llu (x1 %u/%u/%u x2 %u/%u/%u y1 %u y2 %u)\n",
775 stream->dev->name,
776 sof >> 16, div_u64(((u64)sof & 0xffff) * 1000000LLU, 65536),
777 y, timestamp, vbuf->vb2_buf.timestamp,
778 x1, first->host_sof, first->dev_sof,
779 x2, last->host_sof, last->dev_sof, y1, y2);
780
781
782 vbuf->vb2_buf.timestamp = timestamp;
783
784done:
785 spin_unlock_irqrestore(&clock->lock, flags);
786}
787
788
789
790
791
792static void uvc_video_stats_decode(struct uvc_streaming *stream,
793 const u8 *data, int len)
794{
795 unsigned int header_size;
796 bool has_pts = false;
797 bool has_scr = false;
798 u16 scr_sof;
799 u32 scr_stc;
800 u32 pts;
801
802 if (stream->stats.stream.nb_frames == 0 &&
803 stream->stats.frame.nb_packets == 0)
804 stream->stats.stream.start_ts = ktime_get();
805
806 switch (data[1] & (UVC_STREAM_PTS | UVC_STREAM_SCR)) {
807 case UVC_STREAM_PTS | UVC_STREAM_SCR:
808 header_size = 12;
809 has_pts = true;
810 has_scr = true;
811 break;
812 case UVC_STREAM_PTS:
813 header_size = 6;
814 has_pts = true;
815 break;
816 case UVC_STREAM_SCR:
817 header_size = 8;
818 has_scr = true;
819 break;
820 default:
821 header_size = 2;
822 break;
823 }
824
825
826 if (len < header_size || data[0] < header_size) {
827 stream->stats.frame.nb_invalid++;
828 return;
829 }
830
831
832 if (has_pts)
833 pts = get_unaligned_le32(&data[2]);
834
835 if (has_scr) {
836 scr_stc = get_unaligned_le32(&data[header_size - 6]);
837 scr_sof = get_unaligned_le16(&data[header_size - 2]);
838 }
839
840
841 if (has_pts && stream->stats.frame.nb_pts) {
842 if (stream->stats.frame.pts != pts) {
843 stream->stats.frame.nb_pts_diffs++;
844 stream->stats.frame.last_pts_diff =
845 stream->stats.frame.nb_packets;
846 }
847 }
848
849 if (has_pts) {
850 stream->stats.frame.nb_pts++;
851 stream->stats.frame.pts = pts;
852 }
853
854
855
856
857 if (stream->stats.frame.size == 0) {
858 if (len > header_size)
859 stream->stats.frame.has_initial_pts = has_pts;
860 if (len == header_size && has_pts)
861 stream->stats.frame.has_early_pts = true;
862 }
863
864
865 if (has_scr && stream->stats.frame.nb_scr) {
866 if (stream->stats.frame.scr_stc != scr_stc)
867 stream->stats.frame.nb_scr_diffs++;
868 }
869
870 if (has_scr) {
871
872 if (stream->stats.stream.nb_frames > 0 ||
873 stream->stats.frame.nb_scr > 0)
874 stream->stats.stream.scr_sof_count +=
875 (scr_sof - stream->stats.stream.scr_sof) % 2048;
876 stream->stats.stream.scr_sof = scr_sof;
877
878 stream->stats.frame.nb_scr++;
879 stream->stats.frame.scr_stc = scr_stc;
880 stream->stats.frame.scr_sof = scr_sof;
881
882 if (scr_sof < stream->stats.stream.min_sof)
883 stream->stats.stream.min_sof = scr_sof;
884 if (scr_sof > stream->stats.stream.max_sof)
885 stream->stats.stream.max_sof = scr_sof;
886 }
887
888
889 if (stream->stats.frame.size == 0 && len > header_size)
890 stream->stats.frame.first_data = stream->stats.frame.nb_packets;
891
892
893 stream->stats.frame.size += len - header_size;
894
895
896 stream->stats.frame.nb_packets++;
897 if (len <= header_size)
898 stream->stats.frame.nb_empty++;
899
900 if (data[1] & UVC_STREAM_ERR)
901 stream->stats.frame.nb_errors++;
902}
903
904static void uvc_video_stats_update(struct uvc_streaming *stream)
905{
906 struct uvc_stats_frame *frame = &stream->stats.frame;
907
908 uvc_dbg(stream->dev, STATS,
909 "frame %u stats: %u/%u/%u packets, %u/%u/%u pts (%searly %sinitial), %u/%u scr, last pts/stc/sof %u/%u/%u\n",
910 stream->sequence, frame->first_data,
911 frame->nb_packets - frame->nb_empty, frame->nb_packets,
912 frame->nb_pts_diffs, frame->last_pts_diff, frame->nb_pts,
913 frame->has_early_pts ? "" : "!",
914 frame->has_initial_pts ? "" : "!",
915 frame->nb_scr_diffs, frame->nb_scr,
916 frame->pts, frame->scr_stc, frame->scr_sof);
917
918 stream->stats.stream.nb_frames++;
919 stream->stats.stream.nb_packets += stream->stats.frame.nb_packets;
920 stream->stats.stream.nb_empty += stream->stats.frame.nb_empty;
921 stream->stats.stream.nb_errors += stream->stats.frame.nb_errors;
922 stream->stats.stream.nb_invalid += stream->stats.frame.nb_invalid;
923
924 if (frame->has_early_pts)
925 stream->stats.stream.nb_pts_early++;
926 if (frame->has_initial_pts)
927 stream->stats.stream.nb_pts_initial++;
928 if (frame->last_pts_diff <= frame->first_data)
929 stream->stats.stream.nb_pts_constant++;
930 if (frame->nb_scr >= frame->nb_packets - frame->nb_empty)
931 stream->stats.stream.nb_scr_count_ok++;
932 if (frame->nb_scr_diffs + 1 == frame->nb_scr)
933 stream->stats.stream.nb_scr_diffs_ok++;
934
935 memset(&stream->stats.frame, 0, sizeof(stream->stats.frame));
936}
937
938size_t uvc_video_stats_dump(struct uvc_streaming *stream, char *buf,
939 size_t size)
940{
941 unsigned int scr_sof_freq;
942 unsigned int duration;
943 size_t count = 0;
944
945
946
947
948 duration = ktime_ms_delta(stream->stats.stream.stop_ts,
949 stream->stats.stream.start_ts);
950 if (duration != 0)
951 scr_sof_freq = stream->stats.stream.scr_sof_count * 1000
952 / duration;
953 else
954 scr_sof_freq = 0;
955
956 count += scnprintf(buf + count, size - count,
957 "frames: %u\npackets: %u\nempty: %u\n"
958 "errors: %u\ninvalid: %u\n",
959 stream->stats.stream.nb_frames,
960 stream->stats.stream.nb_packets,
961 stream->stats.stream.nb_empty,
962 stream->stats.stream.nb_errors,
963 stream->stats.stream.nb_invalid);
964 count += scnprintf(buf + count, size - count,
965 "pts: %u early, %u initial, %u ok\n",
966 stream->stats.stream.nb_pts_early,
967 stream->stats.stream.nb_pts_initial,
968 stream->stats.stream.nb_pts_constant);
969 count += scnprintf(buf + count, size - count,
970 "scr: %u count ok, %u diff ok\n",
971 stream->stats.stream.nb_scr_count_ok,
972 stream->stats.stream.nb_scr_diffs_ok);
973 count += scnprintf(buf + count, size - count,
974 "sof: %u <= sof <= %u, freq %u.%03u kHz\n",
975 stream->stats.stream.min_sof,
976 stream->stats.stream.max_sof,
977 scr_sof_freq / 1000, scr_sof_freq % 1000);
978
979 return count;
980}
981
982static void uvc_video_stats_start(struct uvc_streaming *stream)
983{
984 memset(&stream->stats, 0, sizeof(stream->stats));
985 stream->stats.stream.min_sof = 2048;
986}
987
988static void uvc_video_stats_stop(struct uvc_streaming *stream)
989{
990 stream->stats.stream.stop_ts = ktime_get();
991}
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032static int uvc_video_decode_start(struct uvc_streaming *stream,
1033 struct uvc_buffer *buf, const u8 *data, int len)
1034{
1035 u8 fid;
1036
1037
1038
1039
1040
1041
1042 if (len < 2 || data[0] < 2 || data[0] > len) {
1043 stream->stats.frame.nb_invalid++;
1044 return -EINVAL;
1045 }
1046
1047 fid = data[1] & UVC_STREAM_FID;
1048
1049
1050
1051
1052 if (stream->last_fid != fid) {
1053 stream->sequence++;
1054 if (stream->sequence)
1055 uvc_video_stats_update(stream);
1056 }
1057
1058 uvc_video_clock_decode(stream, buf, data, len);
1059 uvc_video_stats_decode(stream, data, len);
1060
1061
1062
1063
1064 if (buf == NULL) {
1065 stream->last_fid = fid;
1066 return -ENODATA;
1067 }
1068
1069
1070 if (data[1] & UVC_STREAM_ERR) {
1071 uvc_dbg(stream->dev, FRAME,
1072 "Marking buffer as bad (error bit set)\n");
1073 buf->error = 1;
1074 }
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084 if (buf->state != UVC_BUF_STATE_ACTIVE) {
1085 if (fid == stream->last_fid) {
1086 uvc_dbg(stream->dev, FRAME,
1087 "Dropping payload (out of sync)\n");
1088 if ((stream->dev->quirks & UVC_QUIRK_STREAM_NO_FID) &&
1089 (data[1] & UVC_STREAM_EOF))
1090 stream->last_fid ^= UVC_STREAM_FID;
1091 return -ENODATA;
1092 }
1093
1094 buf->buf.field = V4L2_FIELD_NONE;
1095 buf->buf.sequence = stream->sequence;
1096 buf->buf.vb2_buf.timestamp = ktime_to_ns(uvc_video_get_time());
1097
1098
1099 buf->state = UVC_BUF_STATE_ACTIVE;
1100 }
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117 if (fid != stream->last_fid && buf->bytesused != 0) {
1118 uvc_dbg(stream->dev, FRAME,
1119 "Frame complete (FID bit toggled)\n");
1120 buf->state = UVC_BUF_STATE_READY;
1121 return -EAGAIN;
1122 }
1123
1124 stream->last_fid = fid;
1125
1126 return data[0];
1127}
1128
1129static inline enum dma_data_direction uvc_stream_dir(
1130 struct uvc_streaming *stream)
1131{
1132 if (stream->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
1133 return DMA_FROM_DEVICE;
1134 else
1135 return DMA_TO_DEVICE;
1136}
1137
1138static inline struct device *uvc_stream_to_dmadev(struct uvc_streaming *stream)
1139{
1140 return bus_to_hcd(stream->dev->udev->bus)->self.sysdev;
1141}
1142
1143static int uvc_submit_urb(struct uvc_urb *uvc_urb, gfp_t mem_flags)
1144{
1145
1146 dma_sync_sgtable_for_device(uvc_stream_to_dmadev(uvc_urb->stream),
1147 uvc_urb->sgt,
1148 uvc_stream_dir(uvc_urb->stream));
1149 return usb_submit_urb(uvc_urb->urb, mem_flags);
1150}
1151
1152
1153
1154
1155
1156
1157
1158static void uvc_video_copy_data_work(struct work_struct *work)
1159{
1160 struct uvc_urb *uvc_urb = container_of(work, struct uvc_urb, work);
1161 unsigned int i;
1162 int ret;
1163
1164 for (i = 0; i < uvc_urb->async_operations; i++) {
1165 struct uvc_copy_op *op = &uvc_urb->copy_operations[i];
1166
1167 memcpy(op->dst, op->src, op->len);
1168
1169
1170 uvc_queue_buffer_release(op->buf);
1171 }
1172
1173 ret = uvc_submit_urb(uvc_urb, GFP_KERNEL);
1174 if (ret < 0)
1175 dev_err(&uvc_urb->stream->intf->dev,
1176 "Failed to resubmit video URB (%d).\n", ret);
1177}
1178
1179static void uvc_video_decode_data(struct uvc_urb *uvc_urb,
1180 struct uvc_buffer *buf, const u8 *data, int len)
1181{
1182 unsigned int active_op = uvc_urb->async_operations;
1183 struct uvc_copy_op *op = &uvc_urb->copy_operations[active_op];
1184 unsigned int maxlen;
1185
1186 if (len <= 0)
1187 return;
1188
1189 maxlen = buf->length - buf->bytesused;
1190
1191
1192 kref_get(&buf->ref);
1193
1194 op->buf = buf;
1195 op->src = data;
1196 op->dst = buf->mem + buf->bytesused;
1197 op->len = min_t(unsigned int, len, maxlen);
1198
1199 buf->bytesused += op->len;
1200
1201
1202 if (len > maxlen) {
1203 uvc_dbg(uvc_urb->stream->dev, FRAME,
1204 "Frame complete (overflow)\n");
1205 buf->error = 1;
1206 buf->state = UVC_BUF_STATE_READY;
1207 }
1208
1209 uvc_urb->async_operations++;
1210}
1211
1212static void uvc_video_decode_end(struct uvc_streaming *stream,
1213 struct uvc_buffer *buf, const u8 *data, int len)
1214{
1215
1216 if (data[1] & UVC_STREAM_EOF && buf->bytesused != 0) {
1217 uvc_dbg(stream->dev, FRAME, "Frame complete (EOF found)\n");
1218 if (data[0] == len)
1219 uvc_dbg(stream->dev, FRAME, "EOF in empty payload\n");
1220 buf->state = UVC_BUF_STATE_READY;
1221 if (stream->dev->quirks & UVC_QUIRK_STREAM_NO_FID)
1222 stream->last_fid ^= UVC_STREAM_FID;
1223 }
1224}
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237static int uvc_video_encode_header(struct uvc_streaming *stream,
1238 struct uvc_buffer *buf, u8 *data, int len)
1239{
1240 data[0] = 2;
1241 data[1] = UVC_STREAM_EOH | UVC_STREAM_EOF
1242 | (stream->last_fid & UVC_STREAM_FID);
1243 return 2;
1244}
1245
1246static int uvc_video_encode_data(struct uvc_streaming *stream,
1247 struct uvc_buffer *buf, u8 *data, int len)
1248{
1249 struct uvc_video_queue *queue = &stream->queue;
1250 unsigned int nbytes;
1251 void *mem;
1252
1253
1254 mem = buf->mem + queue->buf_used;
1255 nbytes = min((unsigned int)len, buf->bytesused - queue->buf_used);
1256 nbytes = min(stream->bulk.max_payload_size - stream->bulk.payload_size,
1257 nbytes);
1258 memcpy(data, mem, nbytes);
1259
1260 queue->buf_used += nbytes;
1261
1262 return nbytes;
1263}
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280static void uvc_video_decode_meta(struct uvc_streaming *stream,
1281 struct uvc_buffer *meta_buf,
1282 const u8 *mem, unsigned int length)
1283{
1284 struct uvc_meta_buf *meta;
1285 size_t len_std = 2;
1286 bool has_pts, has_scr;
1287 unsigned long flags;
1288 unsigned int sof;
1289 ktime_t time;
1290 const u8 *scr;
1291
1292 if (!meta_buf || length == 2)
1293 return;
1294
1295 if (meta_buf->length - meta_buf->bytesused <
1296 length + sizeof(meta->ns) + sizeof(meta->sof)) {
1297 meta_buf->error = 1;
1298 return;
1299 }
1300
1301 has_pts = mem[1] & UVC_STREAM_PTS;
1302 has_scr = mem[1] & UVC_STREAM_SCR;
1303
1304 if (has_pts) {
1305 len_std += 4;
1306 scr = mem + 6;
1307 } else {
1308 scr = mem + 2;
1309 }
1310
1311 if (has_scr)
1312 len_std += 6;
1313
1314 if (stream->meta.format == V4L2_META_FMT_UVC)
1315 length = len_std;
1316
1317 if (length == len_std && (!has_scr ||
1318 !memcmp(scr, stream->clock.last_scr, 6)))
1319 return;
1320
1321 meta = (struct uvc_meta_buf *)((u8 *)meta_buf->mem + meta_buf->bytesused);
1322 local_irq_save(flags);
1323 time = uvc_video_get_time();
1324 sof = usb_get_current_frame_number(stream->dev->udev);
1325 local_irq_restore(flags);
1326 put_unaligned(ktime_to_ns(time), &meta->ns);
1327 put_unaligned(sof, &meta->sof);
1328
1329 if (has_scr)
1330 memcpy(stream->clock.last_scr, scr, 6);
1331
1332 memcpy(&meta->length, mem, length);
1333 meta_buf->bytesused += length + sizeof(meta->ns) + sizeof(meta->sof);
1334
1335 uvc_dbg(stream->dev, FRAME,
1336 "%s(): t-sys %lluns, SOF %u, len %u, flags 0x%x, PTS %u, STC %u frame SOF %u\n",
1337 __func__, ktime_to_ns(time), meta->sof, meta->length,
1338 meta->flags,
1339 has_pts ? *(u32 *)meta->buf : 0,
1340 has_scr ? *(u32 *)scr : 0,
1341 has_scr ? *(u32 *)(scr + 4) & 0x7ff : 0);
1342}
1343
1344
1345
1346
1347
1348
1349
1350
1351static void uvc_video_validate_buffer(const struct uvc_streaming *stream,
1352 struct uvc_buffer *buf)
1353{
1354 if (stream->ctrl.dwMaxVideoFrameSize != buf->bytesused &&
1355 !(stream->cur_format->flags & UVC_FMT_FLAG_COMPRESSED))
1356 buf->error = 1;
1357}
1358
1359
1360
1361
1362
1363static void uvc_video_next_buffers(struct uvc_streaming *stream,
1364 struct uvc_buffer **video_buf, struct uvc_buffer **meta_buf)
1365{
1366 uvc_video_validate_buffer(stream, *video_buf);
1367
1368 if (*meta_buf) {
1369 struct vb2_v4l2_buffer *vb2_meta = &(*meta_buf)->buf;
1370 const struct vb2_v4l2_buffer *vb2_video = &(*video_buf)->buf;
1371
1372 vb2_meta->sequence = vb2_video->sequence;
1373 vb2_meta->field = vb2_video->field;
1374 vb2_meta->vb2_buf.timestamp = vb2_video->vb2_buf.timestamp;
1375
1376 (*meta_buf)->state = UVC_BUF_STATE_READY;
1377 if (!(*meta_buf)->error)
1378 (*meta_buf)->error = (*video_buf)->error;
1379 *meta_buf = uvc_queue_next_buffer(&stream->meta.queue,
1380 *meta_buf);
1381 }
1382 *video_buf = uvc_queue_next_buffer(&stream->queue, *video_buf);
1383}
1384
1385static void uvc_video_decode_isoc(struct uvc_urb *uvc_urb,
1386 struct uvc_buffer *buf, struct uvc_buffer *meta_buf)
1387{
1388 struct urb *urb = uvc_urb->urb;
1389 struct uvc_streaming *stream = uvc_urb->stream;
1390 u8 *mem;
1391 int ret, i;
1392
1393 for (i = 0; i < urb->number_of_packets; ++i) {
1394 if (urb->iso_frame_desc[i].status < 0) {
1395 uvc_dbg(stream->dev, FRAME,
1396 "USB isochronous frame lost (%d)\n",
1397 urb->iso_frame_desc[i].status);
1398
1399 if (buf != NULL)
1400 buf->error = 1;
1401 continue;
1402 }
1403
1404
1405 mem = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
1406 do {
1407 ret = uvc_video_decode_start(stream, buf, mem,
1408 urb->iso_frame_desc[i].actual_length);
1409 if (ret == -EAGAIN)
1410 uvc_video_next_buffers(stream, &buf, &meta_buf);
1411 } while (ret == -EAGAIN);
1412
1413 if (ret < 0)
1414 continue;
1415
1416 uvc_video_decode_meta(stream, meta_buf, mem, ret);
1417
1418
1419 uvc_video_decode_data(uvc_urb, buf, mem + ret,
1420 urb->iso_frame_desc[i].actual_length - ret);
1421
1422
1423 uvc_video_decode_end(stream, buf, mem,
1424 urb->iso_frame_desc[i].actual_length);
1425
1426 if (buf->state == UVC_BUF_STATE_READY)
1427 uvc_video_next_buffers(stream, &buf, &meta_buf);
1428 }
1429}
1430
1431static void uvc_video_decode_bulk(struct uvc_urb *uvc_urb,
1432 struct uvc_buffer *buf, struct uvc_buffer *meta_buf)
1433{
1434 struct urb *urb = uvc_urb->urb;
1435 struct uvc_streaming *stream = uvc_urb->stream;
1436 u8 *mem;
1437 int len, ret;
1438
1439
1440
1441
1442
1443 if (urb->actual_length == 0 && stream->bulk.header_size == 0)
1444 return;
1445
1446 mem = urb->transfer_buffer;
1447 len = urb->actual_length;
1448 stream->bulk.payload_size += len;
1449
1450
1451
1452
1453 if (stream->bulk.header_size == 0 && !stream->bulk.skip_payload) {
1454 do {
1455 ret = uvc_video_decode_start(stream, buf, mem, len);
1456 if (ret == -EAGAIN)
1457 uvc_video_next_buffers(stream, &buf, &meta_buf);
1458 } while (ret == -EAGAIN);
1459
1460
1461 if (ret < 0 || buf == NULL) {
1462 stream->bulk.skip_payload = 1;
1463 } else {
1464 memcpy(stream->bulk.header, mem, ret);
1465 stream->bulk.header_size = ret;
1466
1467 uvc_video_decode_meta(stream, meta_buf, mem, ret);
1468
1469 mem += ret;
1470 len -= ret;
1471 }
1472 }
1473
1474
1475
1476
1477
1478
1479
1480 if (!stream->bulk.skip_payload && buf != NULL)
1481 uvc_video_decode_data(uvc_urb, buf, mem, len);
1482
1483
1484
1485
1486 if (urb->actual_length < urb->transfer_buffer_length ||
1487 stream->bulk.payload_size >= stream->bulk.max_payload_size) {
1488 if (!stream->bulk.skip_payload && buf != NULL) {
1489 uvc_video_decode_end(stream, buf, stream->bulk.header,
1490 stream->bulk.payload_size);
1491 if (buf->state == UVC_BUF_STATE_READY)
1492 uvc_video_next_buffers(stream, &buf, &meta_buf);
1493 }
1494
1495 stream->bulk.header_size = 0;
1496 stream->bulk.skip_payload = 0;
1497 stream->bulk.payload_size = 0;
1498 }
1499}
1500
1501static void uvc_video_encode_bulk(struct uvc_urb *uvc_urb,
1502 struct uvc_buffer *buf, struct uvc_buffer *meta_buf)
1503{
1504 struct urb *urb = uvc_urb->urb;
1505 struct uvc_streaming *stream = uvc_urb->stream;
1506
1507 u8 *mem = urb->transfer_buffer;
1508 int len = stream->urb_size, ret;
1509
1510 if (buf == NULL) {
1511 urb->transfer_buffer_length = 0;
1512 return;
1513 }
1514
1515
1516 if (stream->bulk.header_size == 0) {
1517 ret = uvc_video_encode_header(stream, buf, mem, len);
1518 stream->bulk.header_size = ret;
1519 stream->bulk.payload_size += ret;
1520 mem += ret;
1521 len -= ret;
1522 }
1523
1524
1525 ret = uvc_video_encode_data(stream, buf, mem, len);
1526
1527 stream->bulk.payload_size += ret;
1528 len -= ret;
1529
1530 if (buf->bytesused == stream->queue.buf_used ||
1531 stream->bulk.payload_size == stream->bulk.max_payload_size) {
1532 if (buf->bytesused == stream->queue.buf_used) {
1533 stream->queue.buf_used = 0;
1534 buf->state = UVC_BUF_STATE_READY;
1535 buf->buf.sequence = ++stream->sequence;
1536 uvc_queue_next_buffer(&stream->queue, buf);
1537 stream->last_fid ^= UVC_STREAM_FID;
1538 }
1539
1540 stream->bulk.header_size = 0;
1541 stream->bulk.payload_size = 0;
1542 }
1543
1544 urb->transfer_buffer_length = stream->urb_size - len;
1545}
1546
1547static void uvc_video_complete(struct urb *urb)
1548{
1549 struct uvc_urb *uvc_urb = urb->context;
1550 struct uvc_streaming *stream = uvc_urb->stream;
1551 struct uvc_video_queue *queue = &stream->queue;
1552 struct uvc_video_queue *qmeta = &stream->meta.queue;
1553 struct vb2_queue *vb2_qmeta = stream->meta.vdev.queue;
1554 struct uvc_buffer *buf = NULL;
1555 struct uvc_buffer *buf_meta = NULL;
1556 unsigned long flags;
1557 int ret;
1558
1559 switch (urb->status) {
1560 case 0:
1561 break;
1562
1563 default:
1564 dev_warn(&stream->intf->dev,
1565 "Non-zero status (%d) in video completion handler.\n",
1566 urb->status);
1567 fallthrough;
1568 case -ENOENT:
1569 if (stream->frozen)
1570 return;
1571 fallthrough;
1572 case -ECONNRESET:
1573 case -ESHUTDOWN:
1574 uvc_queue_cancel(queue, urb->status == -ESHUTDOWN);
1575 if (vb2_qmeta)
1576 uvc_queue_cancel(qmeta, urb->status == -ESHUTDOWN);
1577 return;
1578 }
1579
1580 buf = uvc_queue_get_current_buffer(queue);
1581
1582 if (vb2_qmeta) {
1583 spin_lock_irqsave(&qmeta->irqlock, flags);
1584 if (!list_empty(&qmeta->irqqueue))
1585 buf_meta = list_first_entry(&qmeta->irqqueue,
1586 struct uvc_buffer, queue);
1587 spin_unlock_irqrestore(&qmeta->irqlock, flags);
1588 }
1589
1590
1591 uvc_urb->async_operations = 0;
1592
1593
1594 dma_sync_sgtable_for_cpu(uvc_stream_to_dmadev(uvc_urb->stream),
1595 uvc_urb->sgt, uvc_stream_dir(stream));
1596 invalidate_kernel_vmap_range(uvc_urb->buffer,
1597 uvc_urb->stream->urb_size);
1598
1599
1600
1601
1602
1603 stream->decode(uvc_urb, buf, buf_meta);
1604
1605
1606 if (!uvc_urb->async_operations) {
1607 ret = uvc_submit_urb(uvc_urb, GFP_ATOMIC);
1608 if (ret < 0)
1609 dev_err(&stream->intf->dev,
1610 "Failed to resubmit video URB (%d).\n", ret);
1611 return;
1612 }
1613
1614 queue_work(stream->async_wq, &uvc_urb->work);
1615}
1616
1617
1618
1619
1620static void uvc_free_urb_buffers(struct uvc_streaming *stream)
1621{
1622 struct device *dma_dev = uvc_stream_to_dmadev(stream);
1623 struct uvc_urb *uvc_urb;
1624
1625 for_each_uvc_urb(uvc_urb, stream) {
1626 if (!uvc_urb->buffer)
1627 continue;
1628
1629 dma_vunmap_noncontiguous(dma_dev, uvc_urb->buffer);
1630 dma_free_noncontiguous(dma_dev, stream->urb_size, uvc_urb->sgt,
1631 uvc_stream_dir(stream));
1632
1633 uvc_urb->buffer = NULL;
1634 uvc_urb->sgt = NULL;
1635 }
1636
1637 stream->urb_size = 0;
1638}
1639
1640static bool uvc_alloc_urb_buffer(struct uvc_streaming *stream,
1641 struct uvc_urb *uvc_urb, gfp_t gfp_flags)
1642{
1643 struct device *dma_dev = uvc_stream_to_dmadev(stream);
1644
1645 uvc_urb->sgt = dma_alloc_noncontiguous(dma_dev, stream->urb_size,
1646 uvc_stream_dir(stream),
1647 gfp_flags, 0);
1648 if (!uvc_urb->sgt)
1649 return false;
1650 uvc_urb->dma = uvc_urb->sgt->sgl->dma_address;
1651
1652 uvc_urb->buffer = dma_vmap_noncontiguous(dma_dev, stream->urb_size,
1653 uvc_urb->sgt);
1654 if (!uvc_urb->buffer) {
1655 dma_free_noncontiguous(dma_dev, stream->urb_size,
1656 uvc_urb->sgt,
1657 uvc_stream_dir(stream));
1658 uvc_urb->sgt = NULL;
1659 return false;
1660 }
1661
1662 return true;
1663}
1664
1665
1666
1667
1668
1669
1670
1671
1672
1673
1674
1675
1676static int uvc_alloc_urb_buffers(struct uvc_streaming *stream,
1677 unsigned int size, unsigned int psize, gfp_t gfp_flags)
1678{
1679 unsigned int npackets;
1680 unsigned int i;
1681
1682
1683 if (stream->urb_size)
1684 return stream->urb_size / psize;
1685
1686
1687
1688
1689 npackets = DIV_ROUND_UP(size, psize);
1690 if (npackets > UVC_MAX_PACKETS)
1691 npackets = UVC_MAX_PACKETS;
1692
1693
1694 for (; npackets > 1; npackets /= 2) {
1695 stream->urb_size = psize * npackets;
1696
1697 for (i = 0; i < UVC_URBS; ++i) {
1698 struct uvc_urb *uvc_urb = &stream->uvc_urb[i];
1699
1700 if (!uvc_alloc_urb_buffer(stream, uvc_urb, gfp_flags)) {
1701 uvc_free_urb_buffers(stream);
1702 break;
1703 }
1704
1705 uvc_urb->stream = stream;
1706 }
1707
1708 if (i == UVC_URBS) {
1709 uvc_dbg(stream->dev, VIDEO,
1710 "Allocated %u URB buffers of %ux%u bytes each\n",
1711 UVC_URBS, npackets, psize);
1712 return npackets;
1713 }
1714 }
1715
1716 uvc_dbg(stream->dev, VIDEO,
1717 "Failed to allocate URB buffers (%u bytes per packet)\n",
1718 psize);
1719 return 0;
1720}
1721
1722
1723
1724
1725static void uvc_video_stop_transfer(struct uvc_streaming *stream,
1726 int free_buffers)
1727{
1728 struct uvc_urb *uvc_urb;
1729
1730 uvc_video_stats_stop(stream);
1731
1732
1733
1734
1735
1736
1737 for_each_uvc_urb(uvc_urb, stream)
1738 usb_poison_urb(uvc_urb->urb);
1739
1740 flush_workqueue(stream->async_wq);
1741
1742 for_each_uvc_urb(uvc_urb, stream) {
1743 usb_free_urb(uvc_urb->urb);
1744 uvc_urb->urb = NULL;
1745 }
1746
1747 if (free_buffers)
1748 uvc_free_urb_buffers(stream);
1749}
1750
1751
1752
1753
1754static unsigned int uvc_endpoint_max_bpi(struct usb_device *dev,
1755 struct usb_host_endpoint *ep)
1756{
1757 u16 psize;
1758 u16 mult;
1759
1760 switch (dev->speed) {
1761 case USB_SPEED_SUPER:
1762 case USB_SPEED_SUPER_PLUS:
1763 return le16_to_cpu(ep->ss_ep_comp.wBytesPerInterval);
1764 case USB_SPEED_HIGH:
1765 psize = usb_endpoint_maxp(&ep->desc);
1766 mult = usb_endpoint_maxp_mult(&ep->desc);
1767 return psize * mult;
1768 case USB_SPEED_WIRELESS:
1769 psize = usb_endpoint_maxp(&ep->desc);
1770 return psize;
1771 default:
1772 psize = usb_endpoint_maxp(&ep->desc);
1773 return psize;
1774 }
1775}
1776
1777
1778
1779
1780
1781static int uvc_init_video_isoc(struct uvc_streaming *stream,
1782 struct usb_host_endpoint *ep, gfp_t gfp_flags)
1783{
1784 struct urb *urb;
1785 struct uvc_urb *uvc_urb;
1786 unsigned int npackets, i;
1787 u16 psize;
1788 u32 size;
1789
1790 psize = uvc_endpoint_max_bpi(stream->dev->udev, ep);
1791 size = stream->ctrl.dwMaxVideoFrameSize;
1792
1793 npackets = uvc_alloc_urb_buffers(stream, size, psize, gfp_flags);
1794 if (npackets == 0)
1795 return -ENOMEM;
1796
1797 size = npackets * psize;
1798
1799 for_each_uvc_urb(uvc_urb, stream) {
1800 urb = usb_alloc_urb(npackets, gfp_flags);
1801 if (urb == NULL) {
1802 uvc_video_stop_transfer(stream, 1);
1803 return -ENOMEM;
1804 }
1805
1806 urb->dev = stream->dev->udev;
1807 urb->context = uvc_urb;
1808 urb->pipe = usb_rcvisocpipe(stream->dev->udev,
1809 ep->desc.bEndpointAddress);
1810 urb->transfer_flags = URB_ISO_ASAP | URB_NO_TRANSFER_DMA_MAP;
1811 urb->transfer_dma = uvc_urb->dma;
1812 urb->interval = ep->desc.bInterval;
1813 urb->transfer_buffer = uvc_urb->buffer;
1814 urb->complete = uvc_video_complete;
1815 urb->number_of_packets = npackets;
1816 urb->transfer_buffer_length = size;
1817
1818 for (i = 0; i < npackets; ++i) {
1819 urb->iso_frame_desc[i].offset = i * psize;
1820 urb->iso_frame_desc[i].length = psize;
1821 }
1822
1823 uvc_urb->urb = urb;
1824 }
1825
1826 return 0;
1827}
1828
1829
1830
1831
1832
1833static int uvc_init_video_bulk(struct uvc_streaming *stream,
1834 struct usb_host_endpoint *ep, gfp_t gfp_flags)
1835{
1836 struct urb *urb;
1837 struct uvc_urb *uvc_urb;
1838 unsigned int npackets, pipe;
1839 u16 psize;
1840 u32 size;
1841
1842 psize = usb_endpoint_maxp(&ep->desc);
1843 size = stream->ctrl.dwMaxPayloadTransferSize;
1844 stream->bulk.max_payload_size = size;
1845
1846 npackets = uvc_alloc_urb_buffers(stream, size, psize, gfp_flags);
1847 if (npackets == 0)
1848 return -ENOMEM;
1849
1850 size = npackets * psize;
1851
1852 if (usb_endpoint_dir_in(&ep->desc))
1853 pipe = usb_rcvbulkpipe(stream->dev->udev,
1854 ep->desc.bEndpointAddress);
1855 else
1856 pipe = usb_sndbulkpipe(stream->dev->udev,
1857 ep->desc.bEndpointAddress);
1858
1859 if (stream->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
1860 size = 0;
1861
1862 for_each_uvc_urb(uvc_urb, stream) {
1863 urb = usb_alloc_urb(0, gfp_flags);
1864 if (urb == NULL) {
1865 uvc_video_stop_transfer(stream, 1);
1866 return -ENOMEM;
1867 }
1868
1869 usb_fill_bulk_urb(urb, stream->dev->udev, pipe, uvc_urb->buffer,
1870 size, uvc_video_complete, uvc_urb);
1871 urb->transfer_flags = URB_NO_TRANSFER_DMA_MAP;
1872 urb->transfer_dma = uvc_urb->dma;
1873
1874 uvc_urb->urb = urb;
1875 }
1876
1877 return 0;
1878}
1879
1880
1881
1882
1883static int uvc_video_start_transfer(struct uvc_streaming *stream,
1884 gfp_t gfp_flags)
1885{
1886 struct usb_interface *intf = stream->intf;
1887 struct usb_host_endpoint *ep;
1888 struct uvc_urb *uvc_urb;
1889 unsigned int i;
1890 int ret;
1891
1892 stream->sequence = -1;
1893 stream->last_fid = -1;
1894 stream->bulk.header_size = 0;
1895 stream->bulk.skip_payload = 0;
1896 stream->bulk.payload_size = 0;
1897
1898 uvc_video_stats_start(stream);
1899
1900 if (intf->num_altsetting > 1) {
1901 struct usb_host_endpoint *best_ep = NULL;
1902 unsigned int best_psize = UINT_MAX;
1903 unsigned int bandwidth;
1904 unsigned int altsetting;
1905 int intfnum = stream->intfnum;
1906
1907
1908 bandwidth = stream->ctrl.dwMaxPayloadTransferSize;
1909
1910 if (bandwidth == 0) {
1911 uvc_dbg(stream->dev, VIDEO,
1912 "Device requested null bandwidth, defaulting to lowest\n");
1913 bandwidth = 1;
1914 } else {
1915 uvc_dbg(stream->dev, VIDEO,
1916 "Device requested %u B/frame bandwidth\n",
1917 bandwidth);
1918 }
1919
1920 for (i = 0; i < intf->num_altsetting; ++i) {
1921 struct usb_host_interface *alts;
1922 unsigned int psize;
1923
1924 alts = &intf->altsetting[i];
1925 ep = uvc_find_endpoint(alts,
1926 stream->header.bEndpointAddress);
1927 if (ep == NULL)
1928 continue;
1929
1930
1931 psize = uvc_endpoint_max_bpi(stream->dev->udev, ep);
1932 if (psize >= bandwidth && psize <= best_psize) {
1933 altsetting = alts->desc.bAlternateSetting;
1934 best_psize = psize;
1935 best_ep = ep;
1936 }
1937 }
1938
1939 if (best_ep == NULL) {
1940 uvc_dbg(stream->dev, VIDEO,
1941 "No fast enough alt setting for requested bandwidth\n");
1942 return -EIO;
1943 }
1944
1945 uvc_dbg(stream->dev, VIDEO,
1946 "Selecting alternate setting %u (%u B/frame bandwidth)\n",
1947 altsetting, best_psize);
1948
1949 ret = usb_set_interface(stream->dev->udev, intfnum, altsetting);
1950 if (ret < 0)
1951 return ret;
1952
1953 ret = uvc_init_video_isoc(stream, best_ep, gfp_flags);
1954 } else {
1955
1956 ep = uvc_find_endpoint(&intf->altsetting[0],
1957 stream->header.bEndpointAddress);
1958 if (ep == NULL)
1959 return -EIO;
1960
1961 ret = uvc_init_video_bulk(stream, ep, gfp_flags);
1962 }
1963
1964 if (ret < 0)
1965 return ret;
1966
1967
1968 for_each_uvc_urb(uvc_urb, stream) {
1969 ret = uvc_submit_urb(uvc_urb, gfp_flags);
1970 if (ret < 0) {
1971 dev_err(&stream->intf->dev,
1972 "Failed to submit URB %u (%d).\n",
1973 uvc_urb_index(uvc_urb), ret);
1974 uvc_video_stop_transfer(stream, 1);
1975 return ret;
1976 }
1977 }
1978
1979
1980
1981
1982 if (stream->dev->quirks & UVC_QUIRK_RESTORE_CTRLS_ON_INIT)
1983 uvc_ctrl_restore_values(stream->dev);
1984
1985 return 0;
1986}
1987
1988
1989
1990
1991
1992
1993
1994
1995
1996
1997
1998
1999int uvc_video_suspend(struct uvc_streaming *stream)
2000{
2001 if (!uvc_queue_streaming(&stream->queue))
2002 return 0;
2003
2004 stream->frozen = 1;
2005 uvc_video_stop_transfer(stream, 0);
2006 usb_set_interface(stream->dev->udev, stream->intfnum, 0);
2007 return 0;
2008}
2009
2010
2011
2012
2013
2014
2015
2016
2017
2018int uvc_video_resume(struct uvc_streaming *stream, int reset)
2019{
2020 int ret;
2021
2022
2023
2024
2025
2026
2027 if (reset)
2028 usb_set_interface(stream->dev->udev, stream->intfnum, 0);
2029
2030 stream->frozen = 0;
2031
2032 uvc_video_clock_reset(stream);
2033
2034 if (!uvc_queue_streaming(&stream->queue))
2035 return 0;
2036
2037 ret = uvc_commit_video(stream, &stream->ctrl);
2038 if (ret < 0)
2039 return ret;
2040
2041 return uvc_video_start_transfer(stream, GFP_NOIO);
2042}
2043
2044
2045
2046
2047
2048
2049
2050
2051
2052
2053
2054
2055
2056
2057
2058int uvc_video_init(struct uvc_streaming *stream)
2059{
2060 struct uvc_streaming_control *probe = &stream->ctrl;
2061 struct uvc_format *format = NULL;
2062 struct uvc_frame *frame = NULL;
2063 struct uvc_urb *uvc_urb;
2064 unsigned int i;
2065 int ret;
2066
2067 if (stream->nformats == 0) {
2068 dev_info(&stream->intf->dev,
2069 "No supported video formats found.\n");
2070 return -EINVAL;
2071 }
2072
2073 atomic_set(&stream->active, 0);
2074
2075
2076
2077
2078
2079
2080 usb_set_interface(stream->dev->udev, stream->intfnum, 0);
2081
2082
2083
2084
2085
2086
2087 if (uvc_get_video_ctrl(stream, probe, 1, UVC_GET_DEF) == 0)
2088 uvc_set_video_ctrl(stream, probe, 1);
2089
2090
2091
2092
2093
2094
2095 ret = uvc_get_video_ctrl(stream, probe, 1, UVC_GET_CUR);
2096 if (ret < 0)
2097 return ret;
2098
2099
2100
2101
2102 for (i = stream->nformats; i > 0; --i) {
2103 format = &stream->format[i-1];
2104 if (format->index == probe->bFormatIndex)
2105 break;
2106 }
2107
2108 if (format->nframes == 0) {
2109 dev_info(&stream->intf->dev,
2110 "No frame descriptor found for the default format.\n");
2111 return -EINVAL;
2112 }
2113
2114
2115
2116
2117
2118
2119 for (i = format->nframes; i > 0; --i) {
2120 frame = &format->frame[i-1];
2121 if (frame->bFrameIndex == probe->bFrameIndex)
2122 break;
2123 }
2124
2125 probe->bFormatIndex = format->index;
2126 probe->bFrameIndex = frame->bFrameIndex;
2127
2128 stream->def_format = format;
2129 stream->cur_format = format;
2130 stream->cur_frame = frame;
2131
2132
2133 if (stream->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
2134 if (stream->dev->quirks & UVC_QUIRK_BUILTIN_ISIGHT)
2135 stream->decode = uvc_video_decode_isight;
2136 else if (stream->intf->num_altsetting > 1)
2137 stream->decode = uvc_video_decode_isoc;
2138 else
2139 stream->decode = uvc_video_decode_bulk;
2140 } else {
2141 if (stream->intf->num_altsetting == 1)
2142 stream->decode = uvc_video_encode_bulk;
2143 else {
2144 dev_info(&stream->intf->dev,
2145 "Isochronous endpoints are not supported for video output devices.\n");
2146 return -EINVAL;
2147 }
2148 }
2149
2150
2151 for_each_uvc_urb(uvc_urb, stream)
2152 INIT_WORK(&uvc_urb->work, uvc_video_copy_data_work);
2153
2154 return 0;
2155}
2156
2157int uvc_video_start_streaming(struct uvc_streaming *stream)
2158{
2159 int ret;
2160
2161 ret = uvc_video_clock_init(stream);
2162 if (ret < 0)
2163 return ret;
2164
2165
2166 ret = uvc_commit_video(stream, &stream->ctrl);
2167 if (ret < 0)
2168 goto error_commit;
2169
2170 ret = uvc_video_start_transfer(stream, GFP_KERNEL);
2171 if (ret < 0)
2172 goto error_video;
2173
2174 return 0;
2175
2176error_video:
2177 usb_set_interface(stream->dev->udev, stream->intfnum, 0);
2178error_commit:
2179 uvc_video_clock_cleanup(stream);
2180
2181 return ret;
2182}
2183
2184void uvc_video_stop_streaming(struct uvc_streaming *stream)
2185{
2186 uvc_video_stop_transfer(stream, 1);
2187
2188 if (stream->intf->num_altsetting > 1) {
2189 usb_set_interface(stream->dev->udev, stream->intfnum, 0);
2190 } else {
2191
2192
2193
2194
2195
2196 unsigned int epnum = stream->header.bEndpointAddress
2197 & USB_ENDPOINT_NUMBER_MASK;
2198 unsigned int dir = stream->header.bEndpointAddress
2199 & USB_ENDPOINT_DIR_MASK;
2200 unsigned int pipe;
2201
2202 pipe = usb_sndbulkpipe(stream->dev->udev, epnum) | dir;
2203 usb_clear_halt(stream->dev->udev, pipe);
2204 }
2205
2206 uvc_video_clock_cleanup(stream);
2207}
2208