1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * dvb-vb2.c - dvb-vb2
4 *
5 * Copyright (C) 2015 Samsung Electronics
6 *
7 * Author: jh1009.sung@samsung.com
8 *
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation.
12 */
13
14 #include <linux/err.h>
15 #include <linux/kernel.h>
16 #include <linux/module.h>
17 #include <linux/mm.h>
18
19 #include <media/dvbdev.h>
20 #include <media/dvb_vb2.h>
21
22 #define DVB_V2_MAX_SIZE (4096 * 188)
23
24 static int vb2_debug;
25 module_param(vb2_debug, int, 0644);
26
27 #define dprintk(level, fmt, arg...) \
28 do { \
29 if (vb2_debug >= level) \
30 pr_info("vb2: %s: " fmt, __func__, ## arg); \
31 } while (0)
32
_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])33 static int _queue_setup(struct vb2_queue *vq,
34 unsigned int *nbuffers, unsigned int *nplanes,
35 unsigned int sizes[], struct device *alloc_devs[])
36 {
37 struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
38
39 ctx->buf_cnt = *nbuffers;
40 *nplanes = 1;
41 sizes[0] = ctx->buf_siz;
42
43 /*
44 * videobuf2-vmalloc allocator is context-less so no need to set
45 * alloc_ctxs array.
46 */
47
48 dprintk(3, "[%s] count=%d, size=%d\n", ctx->name,
49 *nbuffers, sizes[0]);
50
51 return 0;
52 }
53
_buffer_prepare(struct vb2_buffer * vb)54 static int _buffer_prepare(struct vb2_buffer *vb)
55 {
56 struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
57 unsigned long size = ctx->buf_siz;
58
59 if (vb2_plane_size(vb, 0) < size) {
60 dprintk(1, "[%s] data will not fit into plane (%lu < %lu)\n",
61 ctx->name, vb2_plane_size(vb, 0), size);
62 return -EINVAL;
63 }
64
65 vb2_set_plane_payload(vb, 0, size);
66 dprintk(3, "[%s]\n", ctx->name);
67
68 return 0;
69 }
70
_buffer_queue(struct vb2_buffer * vb)71 static void _buffer_queue(struct vb2_buffer *vb)
72 {
73 struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
74 struct dvb_buffer *buf = container_of(vb, struct dvb_buffer, vb);
75 unsigned long flags = 0;
76
77 spin_lock_irqsave(&ctx->slock, flags);
78 list_add_tail(&buf->list, &ctx->dvb_q);
79 spin_unlock_irqrestore(&ctx->slock, flags);
80
81 dprintk(3, "[%s]\n", ctx->name);
82 }
83
_start_streaming(struct vb2_queue * vq,unsigned int count)84 static int _start_streaming(struct vb2_queue *vq, unsigned int count)
85 {
86 struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
87
88 dprintk(3, "[%s] count=%d\n", ctx->name, count);
89 return 0;
90 }
91
_stop_streaming(struct vb2_queue * vq)92 static void _stop_streaming(struct vb2_queue *vq)
93 {
94 struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
95 struct dvb_buffer *buf;
96 unsigned long flags = 0;
97
98 dprintk(3, "[%s]\n", ctx->name);
99
100 spin_lock_irqsave(&ctx->slock, flags);
101 while (!list_empty(&ctx->dvb_q)) {
102 buf = list_entry(ctx->dvb_q.next,
103 struct dvb_buffer, list);
104 vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
105 list_del(&buf->list);
106 }
107 spin_unlock_irqrestore(&ctx->slock, flags);
108 }
109
_dmxdev_lock(struct vb2_queue * vq)110 static void _dmxdev_lock(struct vb2_queue *vq)
111 {
112 struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
113
114 mutex_lock(&ctx->mutex);
115 dprintk(3, "[%s]\n", ctx->name);
116 }
117
_dmxdev_unlock(struct vb2_queue * vq)118 static void _dmxdev_unlock(struct vb2_queue *vq)
119 {
120 struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
121
122 if (mutex_is_locked(&ctx->mutex))
123 mutex_unlock(&ctx->mutex);
124 dprintk(3, "[%s]\n", ctx->name);
125 }
126
127 static const struct vb2_ops dvb_vb2_qops = {
128 .queue_setup = _queue_setup,
129 .buf_prepare = _buffer_prepare,
130 .buf_queue = _buffer_queue,
131 .start_streaming = _start_streaming,
132 .stop_streaming = _stop_streaming,
133 .wait_prepare = _dmxdev_unlock,
134 .wait_finish = _dmxdev_lock,
135 };
136
_fill_dmx_buffer(struct vb2_buffer * vb,void * pb)137 static void _fill_dmx_buffer(struct vb2_buffer *vb, void *pb)
138 {
139 struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
140 struct dmx_buffer *b = pb;
141
142 b->index = vb->index;
143 b->length = vb->planes[0].length;
144 b->bytesused = vb->planes[0].bytesused;
145 b->offset = vb->planes[0].m.offset;
146 dprintk(3, "[%s]\n", ctx->name);
147 }
148
_fill_vb2_buffer(struct vb2_buffer * vb,const void * pb,struct vb2_plane * planes)149 static int _fill_vb2_buffer(struct vb2_buffer *vb,
150 const void *pb, struct vb2_plane *planes)
151 {
152 struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
153
154 planes[0].bytesused = 0;
155 dprintk(3, "[%s]\n", ctx->name);
156
157 return 0;
158 }
159
160 static const struct vb2_buf_ops dvb_vb2_buf_ops = {
161 .fill_user_buffer = _fill_dmx_buffer,
162 .fill_vb2_buffer = _fill_vb2_buffer,
163 };
164
165 /*
166 * Videobuf operations
167 */
dvb_vb2_init(struct dvb_vb2_ctx * ctx,const char * name,int nonblocking)168 int dvb_vb2_init(struct dvb_vb2_ctx *ctx, const char *name, int nonblocking)
169 {
170 struct vb2_queue *q = &ctx->vb_q;
171 int ret;
172
173 memset(ctx, 0, sizeof(struct dvb_vb2_ctx));
174 q->type = DVB_BUF_TYPE_CAPTURE;
175 /**capture type*/
176 q->is_output = 0;
177 /**only mmap is supported currently*/
178 q->io_modes = VB2_MMAP;
179 q->drv_priv = ctx;
180 q->buf_struct_size = sizeof(struct dvb_buffer);
181 q->min_buffers_needed = 1;
182 q->ops = &dvb_vb2_qops;
183 q->mem_ops = &vb2_vmalloc_memops;
184 q->buf_ops = &dvb_vb2_buf_ops;
185 q->num_buffers = 0;
186 ret = vb2_core_queue_init(q);
187 if (ret) {
188 ctx->state = DVB_VB2_STATE_NONE;
189 dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
190 return ret;
191 }
192
193 mutex_init(&ctx->mutex);
194 spin_lock_init(&ctx->slock);
195 INIT_LIST_HEAD(&ctx->dvb_q);
196
197 strlcpy(ctx->name, name, DVB_VB2_NAME_MAX);
198 ctx->nonblocking = nonblocking;
199 ctx->state = DVB_VB2_STATE_INIT;
200
201 dprintk(3, "[%s]\n", ctx->name);
202
203 return 0;
204 }
205
dvb_vb2_release(struct dvb_vb2_ctx * ctx)206 int dvb_vb2_release(struct dvb_vb2_ctx *ctx)
207 {
208 struct vb2_queue *q = (struct vb2_queue *)&ctx->vb_q;
209
210 if (ctx->state & DVB_VB2_STATE_INIT)
211 vb2_core_queue_release(q);
212
213 ctx->state = DVB_VB2_STATE_NONE;
214 dprintk(3, "[%s]\n", ctx->name);
215
216 return 0;
217 }
218
dvb_vb2_stream_on(struct dvb_vb2_ctx * ctx)219 int dvb_vb2_stream_on(struct dvb_vb2_ctx *ctx)
220 {
221 struct vb2_queue *q = &ctx->vb_q;
222 int ret;
223
224 ret = vb2_core_streamon(q, q->type);
225 if (ret) {
226 ctx->state = DVB_VB2_STATE_NONE;
227 dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
228 return ret;
229 }
230 ctx->state |= DVB_VB2_STATE_STREAMON;
231 dprintk(3, "[%s]\n", ctx->name);
232
233 return 0;
234 }
235
dvb_vb2_stream_off(struct dvb_vb2_ctx * ctx)236 int dvb_vb2_stream_off(struct dvb_vb2_ctx *ctx)
237 {
238 struct vb2_queue *q = (struct vb2_queue *)&ctx->vb_q;
239 int ret;
240
241 ctx->state &= ~DVB_VB2_STATE_STREAMON;
242 ret = vb2_core_streamoff(q, q->type);
243 if (ret) {
244 ctx->state = DVB_VB2_STATE_NONE;
245 dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
246 return ret;
247 }
248 dprintk(3, "[%s]\n", ctx->name);
249
250 return 0;
251 }
252
dvb_vb2_is_streaming(struct dvb_vb2_ctx * ctx)253 int dvb_vb2_is_streaming(struct dvb_vb2_ctx *ctx)
254 {
255 return (ctx->state & DVB_VB2_STATE_STREAMON);
256 }
257
dvb_vb2_fill_buffer(struct dvb_vb2_ctx * ctx,const unsigned char * src,int len,enum dmx_buffer_flags * buffer_flags)258 int dvb_vb2_fill_buffer(struct dvb_vb2_ctx *ctx,
259 const unsigned char *src, int len,
260 enum dmx_buffer_flags *buffer_flags)
261 {
262 unsigned long flags = 0;
263 void *vbuf = NULL;
264 int todo = len;
265 unsigned char *psrc = (unsigned char *)src;
266 int ll = 0;
267
268 /*
269 * normal case: This func is called twice from demux driver
270 * one with valid src pointer, second time with NULL pointer
271 */
272 if (!src || !len)
273 return 0;
274 spin_lock_irqsave(&ctx->slock, flags);
275 if (buffer_flags && *buffer_flags) {
276 ctx->flags |= *buffer_flags;
277 *buffer_flags = 0;
278 }
279 while (todo) {
280 if (!ctx->buf) {
281 if (list_empty(&ctx->dvb_q)) {
282 dprintk(3, "[%s] Buffer overflow!!!\n",
283 ctx->name);
284 break;
285 }
286
287 ctx->buf = list_entry(ctx->dvb_q.next,
288 struct dvb_buffer, list);
289 ctx->remain = vb2_plane_size(&ctx->buf->vb, 0);
290 ctx->offset = 0;
291 }
292
293 if (!dvb_vb2_is_streaming(ctx)) {
294 vb2_buffer_done(&ctx->buf->vb, VB2_BUF_STATE_ERROR);
295 list_del(&ctx->buf->list);
296 ctx->buf = NULL;
297 break;
298 }
299
300 /* Fill buffer */
301 ll = min(todo, ctx->remain);
302 vbuf = vb2_plane_vaddr(&ctx->buf->vb, 0);
303 memcpy(vbuf + ctx->offset, psrc, ll);
304 todo -= ll;
305 psrc += ll;
306
307 ctx->remain -= ll;
308 ctx->offset += ll;
309
310 if (ctx->remain == 0) {
311 vb2_buffer_done(&ctx->buf->vb, VB2_BUF_STATE_DONE);
312 list_del(&ctx->buf->list);
313 ctx->buf = NULL;
314 }
315 }
316
317 if (ctx->nonblocking && ctx->buf) {
318 vb2_set_plane_payload(&ctx->buf->vb, 0, ll);
319 vb2_buffer_done(&ctx->buf->vb, VB2_BUF_STATE_DONE);
320 list_del(&ctx->buf->list);
321 ctx->buf = NULL;
322 }
323 spin_unlock_irqrestore(&ctx->slock, flags);
324
325 if (todo)
326 dprintk(1, "[%s] %d bytes are dropped.\n", ctx->name, todo);
327 else
328 dprintk(3, "[%s]\n", ctx->name);
329
330 dprintk(3, "[%s] %d bytes are copied\n", ctx->name, len - todo);
331 return (len - todo);
332 }
333
dvb_vb2_reqbufs(struct dvb_vb2_ctx * ctx,struct dmx_requestbuffers * req)334 int dvb_vb2_reqbufs(struct dvb_vb2_ctx *ctx, struct dmx_requestbuffers *req)
335 {
336 int ret;
337
338 /* Adjust size to a sane value */
339 if (req->size > DVB_V2_MAX_SIZE)
340 req->size = DVB_V2_MAX_SIZE;
341
342 /* FIXME: round req->size to a 188 or 204 multiple */
343
344 ctx->buf_siz = req->size;
345 ctx->buf_cnt = req->count;
346 ret = vb2_core_reqbufs(&ctx->vb_q, VB2_MEMORY_MMAP, &req->count);
347 if (ret) {
348 ctx->state = DVB_VB2_STATE_NONE;
349 dprintk(1, "[%s] count=%d size=%d errno=%d\n", ctx->name,
350 ctx->buf_cnt, ctx->buf_siz, ret);
351 return ret;
352 }
353 ctx->state |= DVB_VB2_STATE_REQBUFS;
354 dprintk(3, "[%s] count=%d size=%d\n", ctx->name,
355 ctx->buf_cnt, ctx->buf_siz);
356
357 return 0;
358 }
359
dvb_vb2_querybuf(struct dvb_vb2_ctx * ctx,struct dmx_buffer * b)360 int dvb_vb2_querybuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)
361 {
362 vb2_core_querybuf(&ctx->vb_q, b->index, b);
363 dprintk(3, "[%s] index=%d\n", ctx->name, b->index);
364 return 0;
365 }
366
dvb_vb2_expbuf(struct dvb_vb2_ctx * ctx,struct dmx_exportbuffer * exp)367 int dvb_vb2_expbuf(struct dvb_vb2_ctx *ctx, struct dmx_exportbuffer *exp)
368 {
369 struct vb2_queue *q = &ctx->vb_q;
370 int ret;
371
372 ret = vb2_core_expbuf(&ctx->vb_q, &exp->fd, q->type, exp->index,
373 0, exp->flags);
374 if (ret) {
375 dprintk(1, "[%s] index=%d errno=%d\n", ctx->name,
376 exp->index, ret);
377 return ret;
378 }
379 dprintk(3, "[%s] index=%d fd=%d\n", ctx->name, exp->index, exp->fd);
380
381 return 0;
382 }
383
dvb_vb2_qbuf(struct dvb_vb2_ctx * ctx,struct dmx_buffer * b)384 int dvb_vb2_qbuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)
385 {
386 int ret;
387
388 ret = vb2_core_qbuf(&ctx->vb_q, b->index, b);
389 if (ret) {
390 dprintk(1, "[%s] index=%d errno=%d\n", ctx->name,
391 b->index, ret);
392 return ret;
393 }
394 dprintk(5, "[%s] index=%d\n", ctx->name, b->index);
395
396 return 0;
397 }
398
dvb_vb2_dqbuf(struct dvb_vb2_ctx * ctx,struct dmx_buffer * b)399 int dvb_vb2_dqbuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)
400 {
401 unsigned long flags;
402 int ret;
403
404 ret = vb2_core_dqbuf(&ctx->vb_q, &b->index, b, ctx->nonblocking);
405 if (ret) {
406 dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
407 return ret;
408 }
409
410 spin_lock_irqsave(&ctx->slock, flags);
411 b->count = ctx->count++;
412 b->flags = ctx->flags;
413 ctx->flags = 0;
414 spin_unlock_irqrestore(&ctx->slock, flags);
415
416 dprintk(5, "[%s] index=%d, count=%d, flags=%d\n",
417 ctx->name, b->index, ctx->count, b->flags);
418
419
420 return 0;
421 }
422
dvb_vb2_mmap(struct dvb_vb2_ctx * ctx,struct vm_area_struct * vma)423 int dvb_vb2_mmap(struct dvb_vb2_ctx *ctx, struct vm_area_struct *vma)
424 {
425 int ret;
426
427 ret = vb2_mmap(&ctx->vb_q, vma);
428 if (ret) {
429 dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
430 return ret;
431 }
432 dprintk(3, "[%s] ret=%d\n", ctx->name, ret);
433
434 return 0;
435 }
436
dvb_vb2_poll(struct dvb_vb2_ctx * ctx,struct file * file,poll_table * wait)437 __poll_t dvb_vb2_poll(struct dvb_vb2_ctx *ctx, struct file *file,
438 poll_table *wait)
439 {
440 dprintk(3, "[%s]\n", ctx->name);
441 return vb2_core_poll(&ctx->vb_q, file, wait);
442 }
443
444