dvb_vb2.c

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