455 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * dvb-vb2.c - dvb-vb24 *5 * Copyright (C) 2015 Samsung Electronics6 *7 * Author: jh1009.sung@samsung.com8 */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 set41 * 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 operations162 */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{254 unsigned long flags = 0;255 void *vbuf = NULL;256 int todo = len;257 unsigned char *psrc = (unsigned char *)src;258 int ll = 0;259 260 /*261 * normal case: This func is called twice from demux driver262 * one with valid src pointer, second time with NULL pointer263 */264 if (!src || !len)265 return 0;266 spin_lock_irqsave(&ctx->slock, flags);267 if (buffer_flags && *buffer_flags) {268 ctx->flags |= *buffer_flags;269 *buffer_flags = 0;270 }271 while (todo) {272 if (!ctx->buf) {273 if (list_empty(&ctx->dvb_q)) {274 dprintk(3, "[%s] Buffer overflow!!!\n",275 ctx->name);276 break;277 }278 279 ctx->buf = list_entry(ctx->dvb_q.next,280 struct dvb_buffer, list);281 ctx->remain = vb2_plane_size(&ctx->buf->vb, 0);282 ctx->offset = 0;283 }284 285 if (!dvb_vb2_is_streaming(ctx)) {286 vb2_buffer_done(&ctx->buf->vb, VB2_BUF_STATE_ERROR);287 list_del(&ctx->buf->list);288 ctx->buf = NULL;289 break;290 }291 292 /* Fill buffer */293 ll = min(todo, ctx->remain);294 vbuf = vb2_plane_vaddr(&ctx->buf->vb, 0);295 memcpy(vbuf + ctx->offset, psrc, ll);296 todo -= ll;297 psrc += ll;298 299 ctx->remain -= ll;300 ctx->offset += ll;301 302 if (ctx->remain == 0) {303 vb2_buffer_done(&ctx->buf->vb, VB2_BUF_STATE_DONE);304 list_del(&ctx->buf->list);305 ctx->buf = NULL;306 }307 }308 309 if (ctx->nonblocking && ctx->buf) {310 vb2_set_plane_payload(&ctx->buf->vb, 0, ll);311 vb2_buffer_done(&ctx->buf->vb, VB2_BUF_STATE_DONE);312 list_del(&ctx->buf->list);313 ctx->buf = NULL;314 }315 spin_unlock_irqrestore(&ctx->slock, flags);316 317 if (todo)318 dprintk(1, "[%s] %d bytes are dropped.\n", ctx->name, todo);319 else320 dprintk(3, "[%s]\n", ctx->name);321 322 dprintk(3, "[%s] %d bytes are copied\n", ctx->name, len - todo);323 return (len - todo);324}325 326int dvb_vb2_reqbufs(struct dvb_vb2_ctx *ctx, struct dmx_requestbuffers *req)327{328 int ret;329 330 /* Adjust size to a sane value */331 if (req->size > DVB_V2_MAX_SIZE)332 req->size = DVB_V2_MAX_SIZE;333 334 /* FIXME: round req->size to a 188 or 204 multiple */335 336 ctx->buf_siz = req->size;337 ctx->buf_cnt = req->count;338 ret = vb2_core_reqbufs(&ctx->vb_q, VB2_MEMORY_MMAP, 0, &req->count);339 if (ret) {340 ctx->state = DVB_VB2_STATE_NONE;341 dprintk(1, "[%s] count=%d size=%d errno=%d\n", ctx->name,342 ctx->buf_cnt, ctx->buf_siz, ret);343 return ret;344 }345 ctx->state |= DVB_VB2_STATE_REQBUFS;346 dprintk(3, "[%s] count=%d size=%d\n", ctx->name,347 ctx->buf_cnt, ctx->buf_siz);348 349 return 0;350}351 352int dvb_vb2_querybuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)353{354 struct vb2_queue *q = &ctx->vb_q;355 struct vb2_buffer *vb2 = vb2_get_buffer(q, b->index);356 357 if (!vb2) {358 dprintk(1, "[%s] invalid buffer index\n", ctx->name);359 return -EINVAL;360 }361 vb2_core_querybuf(&ctx->vb_q, vb2, b);362 dprintk(3, "[%s] index=%d\n", ctx->name, b->index);363 return 0;364}365 366int dvb_vb2_expbuf(struct dvb_vb2_ctx *ctx, struct dmx_exportbuffer *exp)367{368 struct vb2_queue *q = &ctx->vb_q;369 struct vb2_buffer *vb2 = vb2_get_buffer(q, exp->index);370 int ret;371 372 if (!vb2) {373 dprintk(1, "[%s] invalid buffer index\n", ctx->name);374 return -EINVAL;375 }376 377 ret = vb2_core_expbuf(&ctx->vb_q, &exp->fd, q->type, vb2,378 0, exp->flags);379 if (ret) {380 dprintk(1, "[%s] index=%d errno=%d\n", ctx->name,381 exp->index, ret);382 return ret;383 }384 dprintk(3, "[%s] index=%d fd=%d\n", ctx->name, exp->index, exp->fd);385 386 return 0;387}388 389int dvb_vb2_qbuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)390{391 struct vb2_queue *q = &ctx->vb_q;392 struct vb2_buffer *vb2 = vb2_get_buffer(q, b->index);393 int ret;394 395 if (!vb2) {396 dprintk(1, "[%s] invalid buffer index\n", ctx->name);397 return -EINVAL;398 }399 ret = vb2_core_qbuf(&ctx->vb_q, vb2, b, NULL);400 if (ret) {401 dprintk(1, "[%s] index=%d errno=%d\n", ctx->name,402 b->index, ret);403 return ret;404 }405 dprintk(5, "[%s] index=%d\n", ctx->name, b->index);406 407 return 0;408}409 410int dvb_vb2_dqbuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)411{412 unsigned long flags;413 int ret;414 415 ret = vb2_core_dqbuf(&ctx->vb_q, &b->index, b, ctx->nonblocking);416 if (ret) {417 dprintk(1, "[%s] errno=%d\n", ctx->name, ret);418 return ret;419 }420 421 spin_lock_irqsave(&ctx->slock, flags);422 b->count = ctx->count++;423 b->flags = ctx->flags;424 ctx->flags = 0;425 spin_unlock_irqrestore(&ctx->slock, flags);426 427 dprintk(5, "[%s] index=%d, count=%d, flags=%d\n",428 ctx->name, b->index, ctx->count, b->flags);429 430 431 return 0;432}433 434int dvb_vb2_mmap(struct dvb_vb2_ctx *ctx, struct vm_area_struct *vma)435{436 int ret;437 438 ret = vb2_mmap(&ctx->vb_q, vma);439 if (ret) {440 dprintk(1, "[%s] errno=%d\n", ctx->name, ret);441 return ret;442 }443 dprintk(3, "[%s] ret=%d\n", ctx->name, ret);444 445 return 0;446}447 448__poll_t dvb_vb2_poll(struct dvb_vb2_ctx *ctx, struct file *file,449 poll_table *wait)450{451 dprintk(3, "[%s]\n", ctx->name);452 return vb2_core_poll(&ctx->vb_q, file, wait);453}454 455