1
2
3
4
5
6
7
8
9
10
11
12
13
14#include <linux/atomic.h>
15#include <linux/kernel.h>
16#include <linux/mm.h>
17#include <linux/list.h>
18#include <linux/module.h>
19#include <linux/usb.h>
20#include <linux/videodev2.h>
21#include <linux/vmalloc.h>
22#include <linux/wait.h>
23#include <media/videobuf2-v4l2.h>
24#include <media/videobuf2-vmalloc.h>
25
26#include "uvcvideo.h"
27
28
29
30
31
32
33
34
35
36
37
38
39
40static inline struct uvc_streaming *
41uvc_queue_to_stream(struct uvc_video_queue *queue)
42{
43 return container_of(queue, struct uvc_streaming, queue);
44}
45
46static inline struct uvc_buffer *uvc_vbuf_to_buffer(struct vb2_v4l2_buffer *buf)
47{
48 return container_of(buf, struct uvc_buffer, buf);
49}
50
51
52
53
54
55
56static void uvc_queue_return_buffers(struct uvc_video_queue *queue,
57 enum uvc_buffer_state state)
58{
59 enum vb2_buffer_state vb2_state = state == UVC_BUF_STATE_ERROR
60 ? VB2_BUF_STATE_ERROR
61 : VB2_BUF_STATE_QUEUED;
62
63 while (!list_empty(&queue->irqqueue)) {
64 struct uvc_buffer *buf = list_first_entry(&queue->irqqueue,
65 struct uvc_buffer,
66 queue);
67 list_del(&buf->queue);
68 buf->state = state;
69 vb2_buffer_done(&buf->buf.vb2_buf, vb2_state);
70 }
71}
72
73
74
75
76
77static int uvc_queue_setup(struct vb2_queue *vq,
78 unsigned int *nbuffers, unsigned int *nplanes,
79 unsigned int sizes[], struct device *alloc_devs[])
80{
81 struct uvc_video_queue *queue = vb2_get_drv_priv(vq);
82 struct uvc_streaming *stream;
83 unsigned int size;
84
85 switch (vq->type) {
86 case V4L2_BUF_TYPE_META_CAPTURE:
87 size = UVC_METATADA_BUF_SIZE;
88 break;
89
90 default:
91 stream = uvc_queue_to_stream(queue);
92 size = stream->ctrl.dwMaxVideoFrameSize;
93 break;
94 }
95
96
97
98
99
100
101 if (*nplanes)
102 return *nplanes != 1 || sizes[0] < size ? -EINVAL : 0;
103
104 *nplanes = 1;
105 sizes[0] = size;
106 return 0;
107}
108
109static int uvc_buffer_prepare(struct vb2_buffer *vb)
110{
111 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
112 struct uvc_video_queue *queue = vb2_get_drv_priv(vb->vb2_queue);
113 struct uvc_buffer *buf = uvc_vbuf_to_buffer(vbuf);
114
115 if (vb->type == V4L2_BUF_TYPE_VIDEO_OUTPUT &&
116 vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) {
117 uvc_trace(UVC_TRACE_CAPTURE, "[E] Bytes used out of bounds.\n");
118 return -EINVAL;
119 }
120
121 if (unlikely(queue->flags & UVC_QUEUE_DISCONNECTED))
122 return -ENODEV;
123
124 buf->state = UVC_BUF_STATE_QUEUED;
125 buf->error = 0;
126 buf->mem = vb2_plane_vaddr(vb, 0);
127 buf->length = vb2_plane_size(vb, 0);
128 if (vb->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
129 buf->bytesused = 0;
130 else
131 buf->bytesused = vb2_get_plane_payload(vb, 0);
132
133 return 0;
134}
135
136static void uvc_buffer_queue(struct vb2_buffer *vb)
137{
138 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
139 struct uvc_video_queue *queue = vb2_get_drv_priv(vb->vb2_queue);
140 struct uvc_buffer *buf = uvc_vbuf_to_buffer(vbuf);
141 unsigned long flags;
142
143 spin_lock_irqsave(&queue->irqlock, flags);
144 if (likely(!(queue->flags & UVC_QUEUE_DISCONNECTED))) {
145 list_add_tail(&buf->queue, &queue->irqqueue);
146 } else {
147
148
149
150 buf->state = UVC_BUF_STATE_ERROR;
151 vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
152 }
153
154 spin_unlock_irqrestore(&queue->irqlock, flags);
155}
156
157static void uvc_buffer_finish(struct vb2_buffer *vb)
158{
159 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
160 struct uvc_video_queue *queue = vb2_get_drv_priv(vb->vb2_queue);
161 struct uvc_streaming *stream = uvc_queue_to_stream(queue);
162 struct uvc_buffer *buf = uvc_vbuf_to_buffer(vbuf);
163
164 if (vb->state == VB2_BUF_STATE_DONE)
165 uvc_video_clock_update(stream, vbuf, buf);
166}
167
168static int uvc_start_streaming(struct vb2_queue *vq, unsigned int count)
169{
170 struct uvc_video_queue *queue = vb2_get_drv_priv(vq);
171 struct uvc_streaming *stream = uvc_queue_to_stream(queue);
172 unsigned long flags;
173 int ret;
174
175 queue->buf_used = 0;
176
177 ret = uvc_video_enable(stream, 1);
178 if (ret == 0)
179 return 0;
180
181 spin_lock_irqsave(&queue->irqlock, flags);
182 uvc_queue_return_buffers(queue, UVC_BUF_STATE_QUEUED);
183 spin_unlock_irqrestore(&queue->irqlock, flags);
184
185 return ret;
186}
187
188static void uvc_stop_streaming(struct vb2_queue *vq)
189{
190 struct uvc_video_queue *queue = vb2_get_drv_priv(vq);
191 unsigned long flags;
192
193 if (vq->type != V4L2_BUF_TYPE_META_CAPTURE)
194 uvc_video_enable(uvc_queue_to_stream(queue), 0);
195
196 spin_lock_irqsave(&queue->irqlock, flags);
197 uvc_queue_return_buffers(queue, UVC_BUF_STATE_ERROR);
198 spin_unlock_irqrestore(&queue->irqlock, flags);
199}
200
201static const struct vb2_ops uvc_queue_qops = {
202 .queue_setup = uvc_queue_setup,
203 .buf_prepare = uvc_buffer_prepare,
204 .buf_queue = uvc_buffer_queue,
205 .buf_finish = uvc_buffer_finish,
206 .wait_prepare = vb2_ops_wait_prepare,
207 .wait_finish = vb2_ops_wait_finish,
208 .start_streaming = uvc_start_streaming,
209 .stop_streaming = uvc_stop_streaming,
210};
211
212static const struct vb2_ops uvc_meta_queue_qops = {
213 .queue_setup = uvc_queue_setup,
214 .buf_prepare = uvc_buffer_prepare,
215 .buf_queue = uvc_buffer_queue,
216 .wait_prepare = vb2_ops_wait_prepare,
217 .wait_finish = vb2_ops_wait_finish,
218 .stop_streaming = uvc_stop_streaming,
219};
220
221int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
222 int drop_corrupted)
223{
224 int ret;
225
226 queue->queue.type = type;
227 queue->queue.io_modes = VB2_MMAP | VB2_USERPTR;
228 queue->queue.drv_priv = queue;
229 queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
230 queue->queue.mem_ops = &vb2_vmalloc_memops;
231 queue->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
232 | V4L2_BUF_FLAG_TSTAMP_SRC_SOE;
233 queue->queue.lock = &queue->mutex;
234
235 switch (type) {
236 case V4L2_BUF_TYPE_META_CAPTURE:
237 queue->queue.ops = &uvc_meta_queue_qops;
238 break;
239 default:
240 queue->queue.io_modes |= VB2_DMABUF;
241 queue->queue.ops = &uvc_queue_qops;
242 break;
243 }
244
245 ret = vb2_queue_init(&queue->queue);
246 if (ret)
247 return ret;
248
249 mutex_init(&queue->mutex);
250 spin_lock_init(&queue->irqlock);
251 INIT_LIST_HEAD(&queue->irqqueue);
252 queue->flags = drop_corrupted ? UVC_QUEUE_DROP_CORRUPTED : 0;
253
254 return 0;
255}
256
257void uvc_queue_release(struct uvc_video_queue *queue)
258{
259 mutex_lock(&queue->mutex);
260 vb2_queue_release(&queue->queue);
261 mutex_unlock(&queue->mutex);
262}
263
264
265
266
267
268int uvc_request_buffers(struct uvc_video_queue *queue,
269 struct v4l2_requestbuffers *rb)
270{
271 int ret;
272
273 mutex_lock(&queue->mutex);
274 ret = vb2_reqbufs(&queue->queue, rb);
275 mutex_unlock(&queue->mutex);
276
277 return ret ? ret : rb->count;
278}
279
280int uvc_query_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf)
281{
282 int ret;
283
284 mutex_lock(&queue->mutex);
285 ret = vb2_querybuf(&queue->queue, buf);
286 mutex_unlock(&queue->mutex);
287
288 return ret;
289}
290
291int uvc_create_buffers(struct uvc_video_queue *queue,
292 struct v4l2_create_buffers *cb)
293{
294 int ret;
295
296 mutex_lock(&queue->mutex);
297 ret = vb2_create_bufs(&queue->queue, cb);
298 mutex_unlock(&queue->mutex);
299
300 return ret;
301}
302
303int uvc_queue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf)
304{
305 int ret;
306
307 mutex_lock(&queue->mutex);
308 ret = vb2_qbuf(&queue->queue, buf);
309 mutex_unlock(&queue->mutex);
310
311 return ret;
312}
313
314int uvc_export_buffer(struct uvc_video_queue *queue,
315 struct v4l2_exportbuffer *exp)
316{
317 int ret;
318
319 mutex_lock(&queue->mutex);
320 ret = vb2_expbuf(&queue->queue, exp);
321 mutex_unlock(&queue->mutex);
322
323 return ret;
324}
325
326int uvc_dequeue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf,
327 int nonblocking)
328{
329 int ret;
330
331 mutex_lock(&queue->mutex);
332 ret = vb2_dqbuf(&queue->queue, buf, nonblocking);
333 mutex_unlock(&queue->mutex);
334
335 return ret;
336}
337
338int uvc_queue_streamon(struct uvc_video_queue *queue, enum v4l2_buf_type type)
339{
340 int ret;
341
342 mutex_lock(&queue->mutex);
343 ret = vb2_streamon(&queue->queue, type);
344 mutex_unlock(&queue->mutex);
345
346 return ret;
347}
348
349int uvc_queue_streamoff(struct uvc_video_queue *queue, enum v4l2_buf_type type)
350{
351 int ret;
352
353 mutex_lock(&queue->mutex);
354 ret = vb2_streamoff(&queue->queue, type);
355 mutex_unlock(&queue->mutex);
356
357 return ret;
358}
359
360int uvc_queue_mmap(struct uvc_video_queue *queue, struct vm_area_struct *vma)
361{
362 return vb2_mmap(&queue->queue, vma);
363}
364
365#ifndef CONFIG_MMU
366unsigned long uvc_queue_get_unmapped_area(struct uvc_video_queue *queue,
367 unsigned long pgoff)
368{
369 return vb2_get_unmapped_area(&queue->queue, 0, 0, pgoff, 0);
370}
371#endif
372
373__poll_t uvc_queue_poll(struct uvc_video_queue *queue, struct file *file,
374 poll_table *wait)
375{
376 __poll_t ret;
377
378 mutex_lock(&queue->mutex);
379 ret = vb2_poll(&queue->queue, file, wait);
380 mutex_unlock(&queue->mutex);
381
382 return ret;
383}
384
385
386
387
388
389
390
391
392int uvc_queue_allocated(struct uvc_video_queue *queue)
393{
394 int allocated;
395
396 mutex_lock(&queue->mutex);
397 allocated = vb2_is_busy(&queue->queue);
398 mutex_unlock(&queue->mutex);
399
400 return allocated;
401}
402
403
404
405
406
407
408
409
410
411
412
413
414
415void uvc_queue_cancel(struct uvc_video_queue *queue, int disconnect)
416{
417 unsigned long flags;
418
419 spin_lock_irqsave(&queue->irqlock, flags);
420 uvc_queue_return_buffers(queue, UVC_BUF_STATE_ERROR);
421
422
423
424
425
426
427 if (disconnect)
428 queue->flags |= UVC_QUEUE_DISCONNECTED;
429 spin_unlock_irqrestore(&queue->irqlock, flags);
430}
431
432struct uvc_buffer *uvc_queue_next_buffer(struct uvc_video_queue *queue,
433 struct uvc_buffer *buf)
434{
435 struct uvc_buffer *nextbuf;
436 unsigned long flags;
437
438 if ((queue->flags & UVC_QUEUE_DROP_CORRUPTED) && buf->error) {
439 buf->error = 0;
440 buf->state = UVC_BUF_STATE_QUEUED;
441 buf->bytesused = 0;
442 vb2_set_plane_payload(&buf->buf.vb2_buf, 0, 0);
443 return buf;
444 }
445
446 spin_lock_irqsave(&queue->irqlock, flags);
447 list_del(&buf->queue);
448 if (!list_empty(&queue->irqqueue))
449 nextbuf = list_first_entry(&queue->irqqueue, struct uvc_buffer,
450 queue);
451 else
452 nextbuf = NULL;
453 spin_unlock_irqrestore(&queue->irqlock, flags);
454
455 buf->state = buf->error ? UVC_BUF_STATE_ERROR : UVC_BUF_STATE_DONE;
456 vb2_set_plane_payload(&buf->buf.vb2_buf, 0, buf->bytesused);
457 vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_DONE);
458
459 return nextbuf;
460}
461