1 files changed, 430 insertions, 0 deletions
diff --git a/drivers/media/dvb-core/dvb_vb2.c b/drivers/media/dvb-core/dvb_vb2.c
new file mode 100644
index 000000000000..763145d74e83
--- /dev/null
+++ b/drivers/media/dvb-core/dvb_vb2.c
@@ -0,0 +1,430 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * dvb-vb2.c - dvb-vb2
+ *
+ * Copyright (C) 2015 Samsung Electronics
+ *
+ * Author: jh1009.sung@samsung.com
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mm.h>
+
+#include <media/dvbdev.h>
+#include <media/dvb_vb2.h>
+
+#define DVB_V2_MAX_SIZE		(4096 * 188)
+
+static int vb2_debug;
+module_param(vb2_debug, int, 0644);
+
+#define dprintk(level, fmt, arg...)					      \
+	do {								      \
+		if (vb2_debug >= level)					      \
+			pr_info("vb2: %s: " fmt, __func__, ## arg); \
+	} while (0)
+
+static int _queue_setup(struct vb2_queue *vq,
+			unsigned int *nbuffers, unsigned int *nplanes,
+			unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
+
+	ctx->buf_cnt = *nbuffers;
+	*nplanes = 1;
+	sizes[0] = ctx->buf_siz;
+
+	/*
+	 * videobuf2-vmalloc allocator is context-less so no need to set
+	 * alloc_ctxs array.
+	 */
+
+	dprintk(3, "[%s] count=%d, size=%d\n", ctx->name,
+		*nbuffers, sizes[0]);
+
+	return 0;
+}
+
+static int _buffer_prepare(struct vb2_buffer *vb)
+{
+	struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size = ctx->buf_siz;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dprintk(1, "[%s] data will not fit into plane (%lu < %lu)\n",
+			ctx->name, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+	dprintk(3, "[%s]\n", ctx->name);
+
+	return 0;
+}
+
+static void _buffer_queue(struct vb2_buffer *vb)
+{
+	struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct dvb_buffer *buf = container_of(vb, struct dvb_buffer, vb);
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&ctx->slock, flags);
+	list_add_tail(&buf->list, &ctx->dvb_q);
+	spin_unlock_irqrestore(&ctx->slock, flags);
+
+	dprintk(3, "[%s]\n", ctx->name);
+}
+
+static int _start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
+
+	dprintk(3, "[%s] count=%d\n", ctx->name, count);
+	return 0;
+}
+
+static void _stop_streaming(struct vb2_queue *vq)
+{
+	struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
+	struct dvb_buffer *buf;
+	unsigned long flags = 0;
+
+	dprintk(3, "[%s]\n", ctx->name);
+
+	spin_lock_irqsave(&ctx->slock, flags);
+	while (!list_empty(&ctx->dvb_q)) {
+		buf = list_entry(ctx->dvb_q.next,
+				 struct dvb_buffer, list);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+		list_del(&buf->list);
+	}
+	spin_unlock_irqrestore(&ctx->slock, flags);
+}
+
+static void _dmxdev_lock(struct vb2_queue *vq)
+{
+	struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
+
+	mutex_lock(&ctx->mutex);
+	dprintk(3, "[%s]\n", ctx->name);
+}
+
+static void _dmxdev_unlock(struct vb2_queue *vq)
+{
+	struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vq);
+
+	if (mutex_is_locked(&ctx->mutex))
+		mutex_unlock(&ctx->mutex);
+	dprintk(3, "[%s]\n", ctx->name);
+}
+
+static const struct vb2_ops dvb_vb2_qops = {
+	.queue_setup		= _queue_setup,
+	.buf_prepare		= _buffer_prepare,
+	.buf_queue		= _buffer_queue,
+	.start_streaming	= _start_streaming,
+	.stop_streaming		= _stop_streaming,
+	.wait_prepare		= _dmxdev_unlock,
+	.wait_finish		= _dmxdev_lock,
+};
+
+static void _fill_dmx_buffer(struct vb2_buffer *vb, void *pb)
+{
+	struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct dmx_buffer *b = pb;
+
+	b->index = vb->index;
+	b->length = vb->planes[0].length;
+	b->bytesused = vb->planes[0].bytesused;
+	b->offset = vb->planes[0].m.offset;
+	dprintk(3, "[%s]\n", ctx->name);
+}
+
+static int _fill_vb2_buffer(struct vb2_buffer *vb,
+			    const void *pb, struct vb2_plane *planes)
+{
+	struct dvb_vb2_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	planes[0].bytesused = 0;
+	dprintk(3, "[%s]\n", ctx->name);
+
+	return 0;
+}
+
+static const struct vb2_buf_ops dvb_vb2_buf_ops = {
+	.fill_user_buffer	= _fill_dmx_buffer,
+	.fill_vb2_buffer	= _fill_vb2_buffer,
+};
+
+/*
+ * Videobuf operations
+ */
+int dvb_vb2_init(struct dvb_vb2_ctx *ctx, const char *name, int nonblocking)
+{
+	struct vb2_queue *q = &ctx->vb_q;
+	int ret;
+
+	memset(ctx, 0, sizeof(struct dvb_vb2_ctx));
+	q->type = DVB_BUF_TYPE_CAPTURE;
+	/**capture type*/
+	q->is_output = 0;
+	/**only mmap is supported currently*/
+	q->io_modes = VB2_MMAP;
+	q->drv_priv = ctx;
+	q->buf_struct_size = sizeof(struct dvb_buffer);
+	q->min_buffers_needed = 1;
+	q->ops = &dvb_vb2_qops;
+	q->mem_ops = &vb2_vmalloc_memops;
+	q->buf_ops = &dvb_vb2_buf_ops;
+	q->num_buffers = 0;
+	ret = vb2_core_queue_init(q);
+	if (ret) {
+		ctx->state = DVB_VB2_STATE_NONE;
+		dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
+		return ret;
+	}
+
+	mutex_init(&ctx->mutex);
+	spin_lock_init(&ctx->slock);
+	INIT_LIST_HEAD(&ctx->dvb_q);
+
+	strlcpy(ctx->name, name, DVB_VB2_NAME_MAX);
+	ctx->nonblocking = nonblocking;
+	ctx->state = DVB_VB2_STATE_INIT;
+
+	dprintk(3, "[%s]\n", ctx->name);
+
+	return 0;
+}
+
+int dvb_vb2_release(struct dvb_vb2_ctx *ctx)
+{
+	struct vb2_queue *q = (struct vb2_queue *)&ctx->vb_q;
+
+	if (ctx->state & DVB_VB2_STATE_INIT)
+		vb2_core_queue_release(q);
+
+	ctx->state = DVB_VB2_STATE_NONE;
+	dprintk(3, "[%s]\n", ctx->name);
+
+	return 0;
+}
+
+int dvb_vb2_stream_on(struct dvb_vb2_ctx *ctx)
+{
+	struct vb2_queue *q = &ctx->vb_q;
+	int ret;
+
+	ret = vb2_core_streamon(q, q->type);
+	if (ret) {
+		ctx->state = DVB_VB2_STATE_NONE;
+		dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
+		return ret;
+	}
+	ctx->state |= DVB_VB2_STATE_STREAMON;
+	dprintk(3, "[%s]\n", ctx->name);
+
+	return 0;
+}
+
+int dvb_vb2_stream_off(struct dvb_vb2_ctx *ctx)
+{
+	struct vb2_queue *q = (struct vb2_queue *)&ctx->vb_q;
+	int ret;
+
+	ctx->state &= ~DVB_VB2_STATE_STREAMON;
+	ret = vb2_core_streamoff(q, q->type);
+	if (ret) {
+		ctx->state = DVB_VB2_STATE_NONE;
+		dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
+		return ret;
+	}
+	dprintk(3, "[%s]\n", ctx->name);
+
+	return 0;
+}
+
+int dvb_vb2_is_streaming(struct dvb_vb2_ctx *ctx)
+{
+	return (ctx->state & DVB_VB2_STATE_STREAMON);
+}
+
+int dvb_vb2_fill_buffer(struct dvb_vb2_ctx *ctx,
+			const unsigned char *src, int len)
+{
+	unsigned long flags = 0;
+	void *vbuf = NULL;
+	int todo = len;
+	unsigned char *psrc = (unsigned char *)src;
+	int ll = 0;
+
+	dprintk(3, "[%s] %d bytes are rcvd\n", ctx->name, len);
+	if (!src) {
+		dprintk(3, "[%s]:NULL pointer src\n", ctx->name);
+		/**normal case: This func is called twice from demux driver
+		 * once with valid src pointer, second time with NULL pointer
+		 */
+		return 0;
+	}
+	spin_lock_irqsave(&ctx->slock, flags);
+	while (todo) {
+		if (!ctx->buf) {
+			if (list_empty(&ctx->dvb_q)) {
+				dprintk(3, "[%s] Buffer overflow!!!\n",
+					ctx->name);
+				break;
+			}
+
+			ctx->buf = list_entry(ctx->dvb_q.next,
+					      struct dvb_buffer, list);
+			ctx->remain = vb2_plane_size(&ctx->buf->vb, 0);
+			ctx->offset = 0;
+		}
+
+		if (!dvb_vb2_is_streaming(ctx)) {
+			vb2_buffer_done(&ctx->buf->vb, VB2_BUF_STATE_ERROR);
+			list_del(&ctx->buf->list);
+			ctx->buf = NULL;
+			break;
+		}
+
+		/* Fill buffer */
+		ll = min(todo, ctx->remain);
+		vbuf = vb2_plane_vaddr(&ctx->buf->vb, 0);
+		memcpy(vbuf + ctx->offset, psrc, ll);
+		todo -= ll;
+		psrc += ll;
+
+		ctx->remain -= ll;
+		ctx->offset += ll;
+
+		if (ctx->remain == 0) {
+			vb2_buffer_done(&ctx->buf->vb, VB2_BUF_STATE_DONE);
+			list_del(&ctx->buf->list);
+			ctx->buf = NULL;
+		}
+	}
+
+	if (ctx->nonblocking && ctx->buf) {
+		vb2_set_plane_payload(&ctx->buf->vb, 0, ll);
+		vb2_buffer_done(&ctx->buf->vb, VB2_BUF_STATE_DONE);
+		list_del(&ctx->buf->list);
+		ctx->buf = NULL;
+	}
+	spin_unlock_irqrestore(&ctx->slock, flags);
+
+	if (todo)
+		dprintk(1, "[%s] %d bytes are dropped.\n", ctx->name, todo);
+	else
+		dprintk(3, "[%s]\n", ctx->name);
+
+	dprintk(3, "[%s] %d bytes are copied\n", ctx->name, len - todo);
+	return (len - todo);
+}
+
+int dvb_vb2_reqbufs(struct dvb_vb2_ctx *ctx, struct dmx_requestbuffers *req)
+{
+	int ret;
+
+	/* Adjust size to a sane value */
+	if (req->size > DVB_V2_MAX_SIZE)
+		req->size = DVB_V2_MAX_SIZE;
+
+	/* FIXME: round req->size to a 188 or 204 multiple */
+
+	ctx->buf_siz = req->size;
+	ctx->buf_cnt = req->count;
+	ret = vb2_core_reqbufs(&ctx->vb_q, VB2_MEMORY_MMAP, &req->count);
+	if (ret) {
+		ctx->state = DVB_VB2_STATE_NONE;
+		dprintk(1, "[%s] count=%d size=%d errno=%d\n", ctx->name,
+			ctx->buf_cnt, ctx->buf_siz, ret);
+		return ret;
+	}
+	ctx->state |= DVB_VB2_STATE_REQBUFS;
+	dprintk(3, "[%s] count=%d size=%d\n", ctx->name,
+		ctx->buf_cnt, ctx->buf_siz);
+
+	return 0;
+}
+
+int dvb_vb2_querybuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)
+{
+	vb2_core_querybuf(&ctx->vb_q, b->index, b);
+	dprintk(3, "[%s] index=%d\n", ctx->name, b->index);
+	return 0;
+}
+
+int dvb_vb2_expbuf(struct dvb_vb2_ctx *ctx, struct dmx_exportbuffer *exp)
+{
+	struct vb2_queue *q = &ctx->vb_q;
+	int ret;
+
+	ret = vb2_core_expbuf(&ctx->vb_q, &exp->fd, q->type, exp->index,
+			      0, exp->flags);
+	if (ret) {
+		dprintk(1, "[%s] index=%d errno=%d\n", ctx->name,
+			exp->index, ret);
+		return ret;
+	}
+	dprintk(3, "[%s] index=%d fd=%d\n", ctx->name, exp->index, exp->fd);
+
+	return 0;
+}
+
+int dvb_vb2_qbuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)
+{
+	int ret;
+
+	ret = vb2_core_qbuf(&ctx->vb_q, b->index, b);
+	if (ret) {
+		dprintk(1, "[%s] index=%d errno=%d\n", ctx->name,
+			b->index, ret);
+		return ret;
+	}
+	dprintk(5, "[%s] index=%d\n", ctx->name, b->index);
+
+	return 0;
+}
+
+int dvb_vb2_dqbuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)
+{
+	int ret;
+
+	ret = vb2_core_dqbuf(&ctx->vb_q, &b->index, b, ctx->nonblocking);
+	if (ret) {
+		dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
+		return ret;
+	}
+	dprintk(5, "[%s] index=%d\n", ctx->name, b->index);
+
+	return 0;
+}
+
+int dvb_vb2_mmap(struct dvb_vb2_ctx *ctx, struct vm_area_struct *vma)
+{
+	int ret;
+
+	ret = vb2_mmap(&ctx->vb_q, vma);
+	if (ret) {
+		dprintk(1, "[%s] errno=%d\n", ctx->name, ret);
+		return ret;
+	}
+	dprintk(3, "[%s] ret=%d\n", ctx->name, ret);
+
+	return 0;
+}
+
+__poll_t dvb_vb2_poll(struct dvb_vb2_ctx *ctx, struct file *file,
+		      poll_table *wait)
+{
+	dprintk(3, "[%s]\n", ctx->name);
+	return vb2_core_poll(&ctx->vb_q, file, wait);
+}
+