1
2
3
4
5
6
7
8
9#include <linux/atomic.h>
10#include <linux/kernel.h>
11#include <linux/mm.h>
12#include <linux/list.h>
13#include <linux/module.h>
14#include <linux/usb.h>
15#include <linux/videodev2.h>
16#include <linux/vmalloc.h>
17#include <linux/wait.h>
18
19#include <media/v4l2-common.h>
20#include <media/videobuf2-dma-sg.h>
21#include <media/videobuf2-vmalloc.h>
22
23#include "uvc.h"
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41static int uvc_queue_setup(struct vb2_queue *vq,
42 unsigned int *nbuffers, unsigned int *nplanes,
43 unsigned int sizes[], struct device *alloc_devs[])
44{
45 struct uvc_video_queue *queue = vb2_get_drv_priv(vq);
46 struct uvc_video *video = container_of(queue, struct uvc_video, queue);
47 struct usb_composite_dev *cdev = video->uvc->func.config->cdev;
48
49 if (*nbuffers > UVC_MAX_VIDEO_BUFFERS)
50 *nbuffers = UVC_MAX_VIDEO_BUFFERS;
51
52 *nplanes = 1;
53
54 sizes[0] = video->imagesize;
55
56 if (cdev->gadget->speed < USB_SPEED_SUPER)
57 video->uvc_num_requests = 4;
58 else
59 video->uvc_num_requests = 64;
60
61 return 0;
62}
63
64static int uvc_buffer_prepare(struct vb2_buffer *vb)
65{
66 struct uvc_video_queue *queue = vb2_get_drv_priv(vb->vb2_queue);
67 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
68 struct uvc_buffer *buf = container_of(vbuf, struct uvc_buffer, buf);
69
70 if (vb->type == V4L2_BUF_TYPE_VIDEO_OUTPUT &&
71 vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) {
72 uvc_trace(UVC_TRACE_CAPTURE, "[E] Bytes used out of bounds.\n");
73 return -EINVAL;
74 }
75
76 if (unlikely(queue->flags & UVC_QUEUE_DISCONNECTED))
77 return -ENODEV;
78
79 buf->state = UVC_BUF_STATE_QUEUED;
80 if (queue->use_sg) {
81 buf->sgt = vb2_dma_sg_plane_desc(vb, 0);
82 buf->sg = buf->sgt->sgl;
83 } else {
84 buf->mem = vb2_plane_vaddr(vb, 0);
85 }
86 buf->length = vb2_plane_size(vb, 0);
87 if (vb->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
88 buf->bytesused = 0;
89 else
90 buf->bytesused = vb2_get_plane_payload(vb, 0);
91
92 return 0;
93}
94
95static void uvc_buffer_queue(struct vb2_buffer *vb)
96{
97 struct uvc_video_queue *queue = vb2_get_drv_priv(vb->vb2_queue);
98 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
99 struct uvc_buffer *buf = container_of(vbuf, struct uvc_buffer, buf);
100 unsigned long flags;
101
102 spin_lock_irqsave(&queue->irqlock, flags);
103
104 if (likely(!(queue->flags & UVC_QUEUE_DISCONNECTED))) {
105 list_add_tail(&buf->queue, &queue->irqqueue);
106 } else {
107
108
109
110 buf->state = UVC_BUF_STATE_ERROR;
111 vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
112 }
113
114 spin_unlock_irqrestore(&queue->irqlock, flags);
115}
116
117static const struct vb2_ops uvc_queue_qops = {
118 .queue_setup = uvc_queue_setup,
119 .buf_prepare = uvc_buffer_prepare,
120 .buf_queue = uvc_buffer_queue,
121 .wait_prepare = vb2_ops_wait_prepare,
122 .wait_finish = vb2_ops_wait_finish,
123};
124
125int uvcg_queue_init(struct uvc_video_queue *queue, struct device *dev, enum v4l2_buf_type type,
126 struct mutex *lock)
127{
128 struct uvc_video *video = container_of(queue, struct uvc_video, queue);
129 struct usb_composite_dev *cdev = video->uvc->func.config->cdev;
130 int ret;
131
132 queue->queue.type = type;
133 queue->queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
134 queue->queue.drv_priv = queue;
135 queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
136 queue->queue.ops = &uvc_queue_qops;
137 queue->queue.lock = lock;
138 if (cdev->gadget->sg_supported) {
139 queue->queue.mem_ops = &vb2_dma_sg_memops;
140 queue->use_sg = 1;
141 } else {
142 queue->queue.mem_ops = &vb2_vmalloc_memops;
143 }
144
145 queue->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC
146 | V4L2_BUF_FLAG_TSTAMP_SRC_EOF;
147 queue->queue.dev = dev;
148
149 ret = vb2_queue_init(&queue->queue);
150 if (ret)
151 return ret;
152
153 spin_lock_init(&queue->irqlock);
154 INIT_LIST_HEAD(&queue->irqqueue);
155 queue->flags = 0;
156
157 return 0;
158}
159
160
161
162
163void uvcg_free_buffers(struct uvc_video_queue *queue)
164{
165 vb2_queue_release(&queue->queue);
166}
167
168
169
170
171int uvcg_alloc_buffers(struct uvc_video_queue *queue,
172 struct v4l2_requestbuffers *rb)
173{
174 int ret;
175
176 ret = vb2_reqbufs(&queue->queue, rb);
177
178 return ret ? ret : rb->count;
179}
180
181int uvcg_query_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf)
182{
183 return vb2_querybuf(&queue->queue, buf);
184}
185
186int uvcg_queue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf)
187{
188 unsigned long flags;
189 int ret;
190
191 ret = vb2_qbuf(&queue->queue, NULL, buf);
192 if (ret < 0)
193 return ret;
194
195 spin_lock_irqsave(&queue->irqlock, flags);
196 ret = (queue->flags & UVC_QUEUE_PAUSED) != 0;
197 queue->flags &= ~UVC_QUEUE_PAUSED;
198 spin_unlock_irqrestore(&queue->irqlock, flags);
199 return ret;
200}
201
202
203
204
205
206int uvcg_dequeue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf,
207 int nonblocking)
208{
209 return vb2_dqbuf(&queue->queue, buf, nonblocking);
210}
211
212
213
214
215
216
217
218__poll_t uvcg_queue_poll(struct uvc_video_queue *queue, struct file *file,
219 poll_table *wait)
220{
221 return vb2_poll(&queue->queue, file, wait);
222}
223
224int uvcg_queue_mmap(struct uvc_video_queue *queue, struct vm_area_struct *vma)
225{
226 return vb2_mmap(&queue->queue, vma);
227}
228
229#ifndef CONFIG_MMU
230
231
232
233
234
235unsigned long uvcg_queue_get_unmapped_area(struct uvc_video_queue *queue,
236 unsigned long pgoff)
237{
238 return vb2_get_unmapped_area(&queue->queue, 0, 0, pgoff, 0);
239}
240#endif
241
242
243
244
245
246
247
248
249
250
251
252
253
254void uvcg_queue_cancel(struct uvc_video_queue *queue, int disconnect)
255{
256 struct uvc_buffer *buf;
257 unsigned long flags;
258
259 spin_lock_irqsave(&queue->irqlock, flags);
260 while (!list_empty(&queue->irqqueue)) {
261 buf = list_first_entry(&queue->irqqueue, struct uvc_buffer,
262 queue);
263 list_del(&buf->queue);
264 buf->state = UVC_BUF_STATE_ERROR;
265 vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_ERROR);
266 }
267
268
269
270
271
272
273 if (disconnect)
274 queue->flags |= UVC_QUEUE_DISCONNECTED;
275 spin_unlock_irqrestore(&queue->irqlock, flags);
276}
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295int uvcg_queue_enable(struct uvc_video_queue *queue, int enable)
296{
297 unsigned long flags;
298 int ret = 0;
299
300 if (enable) {
301 ret = vb2_streamon(&queue->queue, queue->queue.type);
302 if (ret < 0)
303 return ret;
304
305 queue->sequence = 0;
306 queue->buf_used = 0;
307 } else {
308 ret = vb2_streamoff(&queue->queue, queue->queue.type);
309 if (ret < 0)
310 return ret;
311
312 spin_lock_irqsave(&queue->irqlock, flags);
313 INIT_LIST_HEAD(&queue->irqqueue);
314
315
316
317
318
319
320
321 queue->flags &= ~UVC_QUEUE_DISCONNECTED;
322 spin_unlock_irqrestore(&queue->irqlock, flags);
323 }
324
325 return ret;
326}
327
328
329struct uvc_buffer *uvcg_queue_next_buffer(struct uvc_video_queue *queue,
330 struct uvc_buffer *buf)
331{
332 struct uvc_buffer *nextbuf;
333
334 if ((queue->flags & UVC_QUEUE_DROP_INCOMPLETE) &&
335 buf->length != buf->bytesused) {
336 buf->state = UVC_BUF_STATE_QUEUED;
337 vb2_set_plane_payload(&buf->buf.vb2_buf, 0, 0);
338 return buf;
339 }
340
341 list_del(&buf->queue);
342 if (!list_empty(&queue->irqqueue))
343 nextbuf = list_first_entry(&queue->irqqueue, struct uvc_buffer,
344 queue);
345 else
346 nextbuf = NULL;
347
348 buf->buf.field = V4L2_FIELD_NONE;
349 buf->buf.sequence = queue->sequence++;
350 buf->buf.vb2_buf.timestamp = ktime_get_ns();
351
352 vb2_set_plane_payload(&buf->buf.vb2_buf, 0, buf->bytesused);
353 vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_DONE);
354
355 return nextbuf;
356}
357
358struct uvc_buffer *uvcg_queue_head(struct uvc_video_queue *queue)
359{
360 struct uvc_buffer *buf = NULL;
361
362 if (!list_empty(&queue->irqqueue))
363 buf = list_first_entry(&queue->irqqueue, struct uvc_buffer,
364 queue);
365 else
366 queue->flags |= UVC_QUEUE_PAUSED;
367
368 return buf;
369}
370
371