1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * V4L2 deinterlacing support. 4 * 5 * Copyright (c) 2012 Vista Silicon S.L. 6 * Javier Martin <javier.martin@vista-silicon.com> 7 */ 8 9 #include <linux/module.h> 10 #include <linux/slab.h> 11 #include <linux/interrupt.h> 12 #include <linux/dmaengine.h> 13 #include <linux/platform_device.h> 14 15 #include <media/v4l2-mem2mem.h> 16 #include <media/v4l2-device.h> 17 #include <media/v4l2-ioctl.h> 18 #include <media/videobuf2-dma-contig.h> 19 20 #define MEM2MEM_TEST_MODULE_NAME "mem2mem-deinterlace" 21 22 MODULE_DESCRIPTION("mem2mem device which supports deinterlacing using dmaengine"); 23 MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com"); 24 MODULE_LICENSE("GPL"); 25 MODULE_VERSION("0.0.1"); 26 27 static bool debug; 28 module_param(debug, bool, 0644); 29 30 /* Flags that indicate a format can be used for capture/output */ 31 #define MEM2MEM_CAPTURE (1 << 0) 32 #define MEM2MEM_OUTPUT (1 << 1) 33 34 #define MEM2MEM_NAME "m2m-deinterlace" 35 36 #define dprintk(dev, fmt, arg...) \ 37 v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg) 38 39 struct deinterlace_fmt { 40 char *name; 41 u32 fourcc; 42 /* Types the format can be used for */ 43 u32 types; 44 }; 45 46 static struct deinterlace_fmt formats[] = { 47 { 48 .name = "YUV 4:2:0 Planar", 49 .fourcc = V4L2_PIX_FMT_YUV420, 50 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT, 51 }, 52 { 53 .name = "YUYV 4:2:2", 54 .fourcc = V4L2_PIX_FMT_YUYV, 55 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT, 56 }, 57 }; 58 59 #define NUM_FORMATS ARRAY_SIZE(formats) 60 61 /* Per-queue, driver-specific private data */ 62 struct deinterlace_q_data { 63 unsigned int width; 64 unsigned int height; 65 unsigned int sizeimage; 66 struct deinterlace_fmt *fmt; 67 enum v4l2_field field; 68 }; 69 70 enum { 71 V4L2_M2M_SRC = 0, 72 V4L2_M2M_DST = 1, 73 }; 74 75 enum { 76 YUV420_DMA_Y_ODD, 77 YUV420_DMA_Y_EVEN, 78 YUV420_DMA_U_ODD, 79 YUV420_DMA_U_EVEN, 80 YUV420_DMA_V_ODD, 81 YUV420_DMA_V_EVEN, 82 YUV420_DMA_Y_ODD_DOUBLING, 83 YUV420_DMA_U_ODD_DOUBLING, 84 YUV420_DMA_V_ODD_DOUBLING, 85 YUYV_DMA_ODD, 86 YUYV_DMA_EVEN, 87 YUYV_DMA_EVEN_DOUBLING, 88 }; 89 90 /* Source and destination queue data */ 91 static struct deinterlace_q_data q_data[2]; 92 93 static struct deinterlace_q_data *get_q_data(enum v4l2_buf_type type) 94 { 95 switch (type) { 96 case V4L2_BUF_TYPE_VIDEO_OUTPUT: 97 return &q_data[V4L2_M2M_SRC]; 98 case V4L2_BUF_TYPE_VIDEO_CAPTURE: 99 return &q_data[V4L2_M2M_DST]; 100 default: 101 BUG(); 102 } 103 return NULL; 104 } 105 106 static struct deinterlace_fmt *find_format(struct v4l2_format *f) 107 { 108 struct deinterlace_fmt *fmt; 109 unsigned int k; 110 111 for (k = 0; k < NUM_FORMATS; k++) { 112 fmt = &formats[k]; 113 if ((fmt->types & f->type) && 114 (fmt->fourcc == f->fmt.pix.pixelformat)) 115 break; 116 } 117 118 if (k == NUM_FORMATS) 119 return NULL; 120 121 return &formats[k]; 122 } 123 124 struct deinterlace_dev { 125 struct v4l2_device v4l2_dev; 126 struct video_device vfd; 127 128 atomic_t busy; 129 struct mutex dev_mutex; 130 spinlock_t irqlock; 131 132 struct dma_chan *dma_chan; 133 134 struct v4l2_m2m_dev *m2m_dev; 135 }; 136 137 struct deinterlace_ctx { 138 struct deinterlace_dev *dev; 139 140 /* Abort requested by m2m */ 141 int aborting; 142 enum v4l2_colorspace colorspace; 143 dma_cookie_t cookie; 144 struct v4l2_m2m_ctx *m2m_ctx; 145 struct dma_interleaved_template *xt; 146 }; 147 148 /* 149 * mem2mem callbacks 150 */ 151 static int deinterlace_job_ready(void *priv) 152 { 153 struct deinterlace_ctx *ctx = priv; 154 struct deinterlace_dev *pcdev = ctx->dev; 155 156 if ((v4l2_m2m_num_src_bufs_ready(ctx->m2m_ctx) > 0) 157 && (v4l2_m2m_num_dst_bufs_ready(ctx->m2m_ctx) > 0) 158 && (atomic_read(&ctx->dev->busy) == 0)) { 159 dprintk(pcdev, "Task ready\n"); 160 return 1; 161 } 162 163 dprintk(pcdev, "Task not ready to run\n"); 164 165 return 0; 166 } 167 168 static void deinterlace_job_abort(void *priv) 169 { 170 struct deinterlace_ctx *ctx = priv; 171 struct deinterlace_dev *pcdev = ctx->dev; 172 173 ctx->aborting = 1; 174 175 dprintk(pcdev, "Aborting task\n"); 176 177 v4l2_m2m_job_finish(pcdev->m2m_dev, ctx->m2m_ctx); 178 } 179 180 static void dma_callback(void *data) 181 { 182 struct deinterlace_ctx *curr_ctx = data; 183 struct deinterlace_dev *pcdev = curr_ctx->dev; 184 struct vb2_v4l2_buffer *src_vb, *dst_vb; 185 186 atomic_set(&pcdev->busy, 0); 187 188 src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx); 189 dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx); 190 191 dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp; 192 dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK; 193 dst_vb->flags |= 194 src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK; 195 dst_vb->timecode = src_vb->timecode; 196 197 v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE); 198 v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE); 199 200 v4l2_m2m_job_finish(pcdev->m2m_dev, curr_ctx->m2m_ctx); 201 202 dprintk(pcdev, "dma transfers completed.\n"); 203 } 204 205 static void deinterlace_issue_dma(struct deinterlace_ctx *ctx, int op, 206 int do_callback) 207 { 208 struct deinterlace_q_data *s_q_data; 209 struct vb2_v4l2_buffer *src_buf, *dst_buf; 210 struct deinterlace_dev *pcdev = ctx->dev; 211 struct dma_chan *chan = pcdev->dma_chan; 212 struct dma_device *dmadev = chan->device; 213 struct dma_async_tx_descriptor *tx; 214 unsigned int s_width, s_height; 215 unsigned int s_size; 216 dma_addr_t p_in, p_out; 217 enum dma_ctrl_flags flags; 218 219 src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx); 220 dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx); 221 222 s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT); 223 s_width = s_q_data->width; 224 s_height = s_q_data->height; 225 s_size = s_width * s_height; 226 227 p_in = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0); 228 p_out = (dma_addr_t)vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 229 0); 230 if (!p_in || !p_out) { 231 v4l2_err(&pcdev->v4l2_dev, 232 "Acquiring kernel pointers to buffers failed\n"); 233 return; 234 } 235 236 switch (op) { 237 case YUV420_DMA_Y_ODD: 238 ctx->xt->numf = s_height / 2; 239 ctx->xt->sgl[0].size = s_width; 240 ctx->xt->sgl[0].icg = s_width; 241 ctx->xt->src_start = p_in; 242 ctx->xt->dst_start = p_out; 243 break; 244 case YUV420_DMA_Y_EVEN: 245 ctx->xt->numf = s_height / 2; 246 ctx->xt->sgl[0].size = s_width; 247 ctx->xt->sgl[0].icg = s_width; 248 ctx->xt->src_start = p_in + s_size / 2; 249 ctx->xt->dst_start = p_out + s_width; 250 break; 251 case YUV420_DMA_U_ODD: 252 ctx->xt->numf = s_height / 4; 253 ctx->xt->sgl[0].size = s_width / 2; 254 ctx->xt->sgl[0].icg = s_width / 2; 255 ctx->xt->src_start = p_in + s_size; 256 ctx->xt->dst_start = p_out + s_size; 257 break; 258 case YUV420_DMA_U_EVEN: 259 ctx->xt->numf = s_height / 4; 260 ctx->xt->sgl[0].size = s_width / 2; 261 ctx->xt->sgl[0].icg = s_width / 2; 262 ctx->xt->src_start = p_in + (9 * s_size) / 8; 263 ctx->xt->dst_start = p_out + s_size + s_width / 2; 264 break; 265 case YUV420_DMA_V_ODD: 266 ctx->xt->numf = s_height / 4; 267 ctx->xt->sgl[0].size = s_width / 2; 268 ctx->xt->sgl[0].icg = s_width / 2; 269 ctx->xt->src_start = p_in + (5 * s_size) / 4; 270 ctx->xt->dst_start = p_out + (5 * s_size) / 4; 271 break; 272 case YUV420_DMA_V_EVEN: 273 ctx->xt->numf = s_height / 4; 274 ctx->xt->sgl[0].size = s_width / 2; 275 ctx->xt->sgl[0].icg = s_width / 2; 276 ctx->xt->src_start = p_in + (11 * s_size) / 8; 277 ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2; 278 break; 279 case YUV420_DMA_Y_ODD_DOUBLING: 280 ctx->xt->numf = s_height / 2; 281 ctx->xt->sgl[0].size = s_width; 282 ctx->xt->sgl[0].icg = s_width; 283 ctx->xt->src_start = p_in; 284 ctx->xt->dst_start = p_out + s_width; 285 break; 286 case YUV420_DMA_U_ODD_DOUBLING: 287 ctx->xt->numf = s_height / 4; 288 ctx->xt->sgl[0].size = s_width / 2; 289 ctx->xt->sgl[0].icg = s_width / 2; 290 ctx->xt->src_start = p_in + s_size; 291 ctx->xt->dst_start = p_out + s_size + s_width / 2; 292 break; 293 case YUV420_DMA_V_ODD_DOUBLING: 294 ctx->xt->numf = s_height / 4; 295 ctx->xt->sgl[0].size = s_width / 2; 296 ctx->xt->sgl[0].icg = s_width / 2; 297 ctx->xt->src_start = p_in + (5 * s_size) / 4; 298 ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2; 299 break; 300 case YUYV_DMA_ODD: 301 ctx->xt->numf = s_height / 2; 302 ctx->xt->sgl[0].size = s_width * 2; 303 ctx->xt->sgl[0].icg = s_width * 2; 304 ctx->xt->src_start = p_in; 305 ctx->xt->dst_start = p_out; 306 break; 307 case YUYV_DMA_EVEN: 308 ctx->xt->numf = s_height / 2; 309 ctx->xt->sgl[0].size = s_width * 2; 310 ctx->xt->sgl[0].icg = s_width * 2; 311 ctx->xt->src_start = p_in + s_size; 312 ctx->xt->dst_start = p_out + s_width * 2; 313 break; 314 case YUYV_DMA_EVEN_DOUBLING: 315 default: 316 ctx->xt->numf = s_height / 2; 317 ctx->xt->sgl[0].size = s_width * 2; 318 ctx->xt->sgl[0].icg = s_width * 2; 319 ctx->xt->src_start = p_in; 320 ctx->xt->dst_start = p_out + s_width * 2; 321 break; 322 } 323 324 /* Common parameters for al transfers */ 325 ctx->xt->frame_size = 1; 326 ctx->xt->dir = DMA_MEM_TO_MEM; 327 ctx->xt->src_sgl = false; 328 ctx->xt->dst_sgl = true; 329 flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT; 330 331 tx = dmadev->device_prep_interleaved_dma(chan, ctx->xt, flags); 332 if (tx == NULL) { 333 v4l2_warn(&pcdev->v4l2_dev, "DMA interleaved prep error\n"); 334 return; 335 } 336 337 if (do_callback) { 338 tx->callback = dma_callback; 339 tx->callback_param = ctx; 340 } 341 342 ctx->cookie = dmaengine_submit(tx); 343 if (dma_submit_error(ctx->cookie)) { 344 v4l2_warn(&pcdev->v4l2_dev, 345 "DMA submit error %d with src=0x%x dst=0x%x len=0x%x\n", 346 ctx->cookie, (unsigned)p_in, (unsigned)p_out, 347 s_size * 3/2); 348 return; 349 } 350 351 dma_async_issue_pending(chan); 352 } 353 354 static void deinterlace_device_run(void *priv) 355 { 356 struct deinterlace_ctx *ctx = priv; 357 struct deinterlace_q_data *dst_q_data; 358 359 atomic_set(&ctx->dev->busy, 1); 360 361 dprintk(ctx->dev, "%s: DMA try issue.\n", __func__); 362 363 dst_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE); 364 365 /* 366 * 4 possible field conversions are possible at the moment: 367 * V4L2_FIELD_SEQ_TB --> V4L2_FIELD_INTERLACED_TB: 368 * two separate fields in the same input buffer are interlaced 369 * in the output buffer using weaving. Top field comes first. 370 * V4L2_FIELD_SEQ_TB --> V4L2_FIELD_NONE: 371 * top field from the input buffer is copied to the output buffer 372 * using line doubling. Bottom field from the input buffer is discarded. 373 * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_INTERLACED_BT: 374 * two separate fields in the same input buffer are interlaced 375 * in the output buffer using weaving. Bottom field comes first. 376 * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_NONE: 377 * bottom field from the input buffer is copied to the output buffer 378 * using line doubling. Top field from the input buffer is discarded. 379 */ 380 switch (dst_q_data->fmt->fourcc) { 381 case V4L2_PIX_FMT_YUV420: 382 switch (dst_q_data->field) { 383 case V4L2_FIELD_INTERLACED_TB: 384 case V4L2_FIELD_INTERLACED_BT: 385 dprintk(ctx->dev, "%s: yuv420 interlaced tb.\n", 386 __func__); 387 deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0); 388 deinterlace_issue_dma(ctx, YUV420_DMA_Y_EVEN, 0); 389 deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0); 390 deinterlace_issue_dma(ctx, YUV420_DMA_U_EVEN, 0); 391 deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0); 392 deinterlace_issue_dma(ctx, YUV420_DMA_V_EVEN, 1); 393 break; 394 case V4L2_FIELD_NONE: 395 default: 396 dprintk(ctx->dev, "%s: yuv420 interlaced line doubling.\n", 397 __func__); 398 deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD, 0); 399 deinterlace_issue_dma(ctx, YUV420_DMA_Y_ODD_DOUBLING, 0); 400 deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD, 0); 401 deinterlace_issue_dma(ctx, YUV420_DMA_U_ODD_DOUBLING, 0); 402 deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD, 0); 403 deinterlace_issue_dma(ctx, YUV420_DMA_V_ODD_DOUBLING, 1); 404 break; 405 } 406 break; 407 case V4L2_PIX_FMT_YUYV: 408 default: 409 switch (dst_q_data->field) { 410 case V4L2_FIELD_INTERLACED_TB: 411 case V4L2_FIELD_INTERLACED_BT: 412 dprintk(ctx->dev, "%s: yuyv interlaced_tb.\n", 413 __func__); 414 deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0); 415 deinterlace_issue_dma(ctx, YUYV_DMA_EVEN, 1); 416 break; 417 case V4L2_FIELD_NONE: 418 default: 419 dprintk(ctx->dev, "%s: yuyv interlaced line doubling.\n", 420 __func__); 421 deinterlace_issue_dma(ctx, YUYV_DMA_ODD, 0); 422 deinterlace_issue_dma(ctx, YUYV_DMA_EVEN_DOUBLING, 1); 423 break; 424 } 425 break; 426 } 427 428 dprintk(ctx->dev, "%s: DMA issue done.\n", __func__); 429 } 430 431 /* 432 * video ioctls 433 */ 434 static int vidioc_querycap(struct file *file, void *priv, 435 struct v4l2_capability *cap) 436 { 437 strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver)); 438 strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card)); 439 strscpy(cap->bus_info, MEM2MEM_NAME, sizeof(cap->card)); 440 /* 441 * This is only a mem-to-mem video device. The capture and output 442 * device capability flags are left only for backward compatibility 443 * and are scheduled for removal. 444 */ 445 cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT | 446 V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING; 447 cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS; 448 449 return 0; 450 } 451 452 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type) 453 { 454 int i, num; 455 struct deinterlace_fmt *fmt; 456 457 num = 0; 458 459 for (i = 0; i < NUM_FORMATS; ++i) { 460 if (formats[i].types & type) { 461 /* index-th format of type type found ? */ 462 if (num == f->index) 463 break; 464 /* Correct type but haven't reached our index yet, 465 * just increment per-type index */ 466 ++num; 467 } 468 } 469 470 if (i < NUM_FORMATS) { 471 /* Format found */ 472 fmt = &formats[i]; 473 strscpy(f->description, fmt->name, sizeof(f->description)); 474 f->pixelformat = fmt->fourcc; 475 return 0; 476 } 477 478 /* Format not found */ 479 return -EINVAL; 480 } 481 482 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv, 483 struct v4l2_fmtdesc *f) 484 { 485 return enum_fmt(f, MEM2MEM_CAPTURE); 486 } 487 488 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv, 489 struct v4l2_fmtdesc *f) 490 { 491 return enum_fmt(f, MEM2MEM_OUTPUT); 492 } 493 494 static int vidioc_g_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f) 495 { 496 struct vb2_queue *vq; 497 struct deinterlace_q_data *q_data; 498 499 vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type); 500 if (!vq) 501 return -EINVAL; 502 503 q_data = get_q_data(f->type); 504 505 f->fmt.pix.width = q_data->width; 506 f->fmt.pix.height = q_data->height; 507 f->fmt.pix.field = q_data->field; 508 f->fmt.pix.pixelformat = q_data->fmt->fourcc; 509 510 switch (q_data->fmt->fourcc) { 511 case V4L2_PIX_FMT_YUV420: 512 f->fmt.pix.bytesperline = q_data->width * 3 / 2; 513 break; 514 case V4L2_PIX_FMT_YUYV: 515 default: 516 f->fmt.pix.bytesperline = q_data->width * 2; 517 } 518 519 f->fmt.pix.sizeimage = q_data->sizeimage; 520 f->fmt.pix.colorspace = ctx->colorspace; 521 522 return 0; 523 } 524 525 static int vidioc_g_fmt_vid_out(struct file *file, void *priv, 526 struct v4l2_format *f) 527 { 528 return vidioc_g_fmt(priv, f); 529 } 530 531 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv, 532 struct v4l2_format *f) 533 { 534 return vidioc_g_fmt(priv, f); 535 } 536 537 static int vidioc_try_fmt(struct v4l2_format *f, struct deinterlace_fmt *fmt) 538 { 539 switch (f->fmt.pix.pixelformat) { 540 case V4L2_PIX_FMT_YUV420: 541 f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2; 542 break; 543 case V4L2_PIX_FMT_YUYV: 544 default: 545 f->fmt.pix.bytesperline = f->fmt.pix.width * 2; 546 } 547 f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline; 548 549 return 0; 550 } 551 552 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv, 553 struct v4l2_format *f) 554 { 555 struct deinterlace_fmt *fmt; 556 struct deinterlace_ctx *ctx = priv; 557 558 fmt = find_format(f); 559 if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) 560 f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420; 561 562 f->fmt.pix.colorspace = ctx->colorspace; 563 564 if (f->fmt.pix.field != V4L2_FIELD_INTERLACED_TB && 565 f->fmt.pix.field != V4L2_FIELD_INTERLACED_BT && 566 f->fmt.pix.field != V4L2_FIELD_NONE) 567 f->fmt.pix.field = V4L2_FIELD_INTERLACED_TB; 568 569 return vidioc_try_fmt(f, fmt); 570 } 571 572 static int vidioc_try_fmt_vid_out(struct file *file, void *priv, 573 struct v4l2_format *f) 574 { 575 struct deinterlace_fmt *fmt; 576 577 fmt = find_format(f); 578 if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) 579 f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420; 580 581 if (!f->fmt.pix.colorspace) 582 f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709; 583 584 if (f->fmt.pix.field != V4L2_FIELD_SEQ_TB && 585 f->fmt.pix.field != V4L2_FIELD_SEQ_BT) 586 f->fmt.pix.field = V4L2_FIELD_SEQ_TB; 587 588 return vidioc_try_fmt(f, fmt); 589 } 590 591 static int vidioc_s_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f) 592 { 593 struct deinterlace_q_data *q_data; 594 struct vb2_queue *vq; 595 596 vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type); 597 if (!vq) 598 return -EINVAL; 599 600 q_data = get_q_data(f->type); 601 if (!q_data) 602 return -EINVAL; 603 604 if (vb2_is_busy(vq)) { 605 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__); 606 return -EBUSY; 607 } 608 609 q_data->fmt = find_format(f); 610 if (!q_data->fmt) { 611 v4l2_err(&ctx->dev->v4l2_dev, 612 "Couldn't set format type %d, wxh: %dx%d. fmt: %d, field: %d\n", 613 f->type, f->fmt.pix.width, f->fmt.pix.height, 614 f->fmt.pix.pixelformat, f->fmt.pix.field); 615 return -EINVAL; 616 } 617 618 q_data->width = f->fmt.pix.width; 619 q_data->height = f->fmt.pix.height; 620 q_data->field = f->fmt.pix.field; 621 622 switch (f->fmt.pix.pixelformat) { 623 case V4L2_PIX_FMT_YUV420: 624 f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2; 625 q_data->sizeimage = (q_data->width * q_data->height * 3) / 2; 626 break; 627 case V4L2_PIX_FMT_YUYV: 628 default: 629 f->fmt.pix.bytesperline = f->fmt.pix.width * 2; 630 q_data->sizeimage = q_data->width * q_data->height * 2; 631 } 632 633 dprintk(ctx->dev, 634 "Setting format for type %d, wxh: %dx%d, fmt: %d, field: %d\n", 635 f->type, q_data->width, q_data->height, q_data->fmt->fourcc, 636 q_data->field); 637 638 return 0; 639 } 640 641 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv, 642 struct v4l2_format *f) 643 { 644 int ret; 645 646 ret = vidioc_try_fmt_vid_cap(file, priv, f); 647 if (ret) 648 return ret; 649 return vidioc_s_fmt(priv, f); 650 } 651 652 static int vidioc_s_fmt_vid_out(struct file *file, void *priv, 653 struct v4l2_format *f) 654 { 655 struct deinterlace_ctx *ctx = priv; 656 int ret; 657 658 ret = vidioc_try_fmt_vid_out(file, priv, f); 659 if (ret) 660 return ret; 661 662 ret = vidioc_s_fmt(priv, f); 663 if (!ret) 664 ctx->colorspace = f->fmt.pix.colorspace; 665 666 return ret; 667 } 668 669 static int vidioc_reqbufs(struct file *file, void *priv, 670 struct v4l2_requestbuffers *reqbufs) 671 { 672 struct deinterlace_ctx *ctx = priv; 673 674 return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs); 675 } 676 677 static int vidioc_querybuf(struct file *file, void *priv, 678 struct v4l2_buffer *buf) 679 { 680 struct deinterlace_ctx *ctx = priv; 681 682 return v4l2_m2m_querybuf(file, ctx->m2m_ctx, buf); 683 } 684 685 static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf) 686 { 687 struct deinterlace_ctx *ctx = priv; 688 689 return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf); 690 } 691 692 static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf) 693 { 694 struct deinterlace_ctx *ctx = priv; 695 696 return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf); 697 } 698 699 static int vidioc_streamon(struct file *file, void *priv, 700 enum v4l2_buf_type type) 701 { 702 struct deinterlace_q_data *s_q_data, *d_q_data; 703 struct deinterlace_ctx *ctx = priv; 704 705 s_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_OUTPUT); 706 d_q_data = get_q_data(V4L2_BUF_TYPE_VIDEO_CAPTURE); 707 708 /* Check that src and dst queues have the same pix format */ 709 if (s_q_data->fmt->fourcc != d_q_data->fmt->fourcc) { 710 v4l2_err(&ctx->dev->v4l2_dev, 711 "src and dst formats don't match.\n"); 712 return -EINVAL; 713 } 714 715 /* Check that input and output deinterlacing types are compatible */ 716 switch (s_q_data->field) { 717 case V4L2_FIELD_SEQ_BT: 718 if (d_q_data->field != V4L2_FIELD_NONE && 719 d_q_data->field != V4L2_FIELD_INTERLACED_BT) { 720 v4l2_err(&ctx->dev->v4l2_dev, 721 "src and dst field conversion [(%d)->(%d)] not supported.\n", 722 s_q_data->field, d_q_data->field); 723 return -EINVAL; 724 } 725 break; 726 case V4L2_FIELD_SEQ_TB: 727 if (d_q_data->field != V4L2_FIELD_NONE && 728 d_q_data->field != V4L2_FIELD_INTERLACED_TB) { 729 v4l2_err(&ctx->dev->v4l2_dev, 730 "src and dst field conversion [(%d)->(%d)] not supported.\n", 731 s_q_data->field, d_q_data->field); 732 return -EINVAL; 733 } 734 break; 735 default: 736 return -EINVAL; 737 } 738 739 return v4l2_m2m_streamon(file, ctx->m2m_ctx, type); 740 } 741 742 static int vidioc_streamoff(struct file *file, void *priv, 743 enum v4l2_buf_type type) 744 { 745 struct deinterlace_ctx *ctx = priv; 746 747 return v4l2_m2m_streamoff(file, ctx->m2m_ctx, type); 748 } 749 750 static const struct v4l2_ioctl_ops deinterlace_ioctl_ops = { 751 .vidioc_querycap = vidioc_querycap, 752 753 .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap, 754 .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap, 755 .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap, 756 .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap, 757 758 .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out, 759 .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out, 760 .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out, 761 .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out, 762 763 .vidioc_reqbufs = vidioc_reqbufs, 764 .vidioc_querybuf = vidioc_querybuf, 765 766 .vidioc_qbuf = vidioc_qbuf, 767 .vidioc_dqbuf = vidioc_dqbuf, 768 769 .vidioc_streamon = vidioc_streamon, 770 .vidioc_streamoff = vidioc_streamoff, 771 }; 772 773 774 /* 775 * Queue operations 776 */ 777 struct vb2_dc_conf { 778 struct device *dev; 779 }; 780 781 static int deinterlace_queue_setup(struct vb2_queue *vq, 782 unsigned int *nbuffers, unsigned int *nplanes, 783 unsigned int sizes[], struct device *alloc_devs[]) 784 { 785 struct deinterlace_ctx *ctx = vb2_get_drv_priv(vq); 786 struct deinterlace_q_data *q_data; 787 unsigned int size, count = *nbuffers; 788 789 q_data = get_q_data(vq->type); 790 791 switch (q_data->fmt->fourcc) { 792 case V4L2_PIX_FMT_YUV420: 793 size = q_data->width * q_data->height * 3 / 2; 794 break; 795 case V4L2_PIX_FMT_YUYV: 796 default: 797 size = q_data->width * q_data->height * 2; 798 } 799 800 *nplanes = 1; 801 *nbuffers = count; 802 sizes[0] = size; 803 804 dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size); 805 806 return 0; 807 } 808 809 static int deinterlace_buf_prepare(struct vb2_buffer *vb) 810 { 811 struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 812 struct deinterlace_q_data *q_data; 813 814 dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type); 815 816 q_data = get_q_data(vb->vb2_queue->type); 817 818 if (vb2_plane_size(vb, 0) < q_data->sizeimage) { 819 dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n", 820 __func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage); 821 return -EINVAL; 822 } 823 824 vb2_set_plane_payload(vb, 0, q_data->sizeimage); 825 826 return 0; 827 } 828 829 static void deinterlace_buf_queue(struct vb2_buffer *vb) 830 { 831 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 832 struct deinterlace_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 833 834 v4l2_m2m_buf_queue(ctx->m2m_ctx, vbuf); 835 } 836 837 static const struct vb2_ops deinterlace_qops = { 838 .queue_setup = deinterlace_queue_setup, 839 .buf_prepare = deinterlace_buf_prepare, 840 .buf_queue = deinterlace_buf_queue, 841 .wait_prepare = vb2_ops_wait_prepare, 842 .wait_finish = vb2_ops_wait_finish, 843 }; 844 845 static int queue_init(void *priv, struct vb2_queue *src_vq, 846 struct vb2_queue *dst_vq) 847 { 848 struct deinterlace_ctx *ctx = priv; 849 int ret; 850 851 src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT; 852 src_vq->io_modes = VB2_MMAP | VB2_USERPTR; 853 src_vq->drv_priv = ctx; 854 src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); 855 src_vq->ops = &deinterlace_qops; 856 src_vq->mem_ops = &vb2_dma_contig_memops; 857 src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; 858 src_vq->dev = ctx->dev->v4l2_dev.dev; 859 src_vq->lock = &ctx->dev->dev_mutex; 860 q_data[V4L2_M2M_SRC].fmt = &formats[0]; 861 q_data[V4L2_M2M_SRC].width = 640; 862 q_data[V4L2_M2M_SRC].height = 480; 863 q_data[V4L2_M2M_SRC].sizeimage = (640 * 480 * 3) / 2; 864 q_data[V4L2_M2M_SRC].field = V4L2_FIELD_SEQ_TB; 865 866 ret = vb2_queue_init(src_vq); 867 if (ret) 868 return ret; 869 870 dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 871 dst_vq->io_modes = VB2_MMAP | VB2_USERPTR; 872 dst_vq->drv_priv = ctx; 873 dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); 874 dst_vq->ops = &deinterlace_qops; 875 dst_vq->mem_ops = &vb2_dma_contig_memops; 876 dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; 877 dst_vq->dev = ctx->dev->v4l2_dev.dev; 878 dst_vq->lock = &ctx->dev->dev_mutex; 879 q_data[V4L2_M2M_DST].fmt = &formats[0]; 880 q_data[V4L2_M2M_DST].width = 640; 881 q_data[V4L2_M2M_DST].height = 480; 882 q_data[V4L2_M2M_DST].sizeimage = (640 * 480 * 3) / 2; 883 q_data[V4L2_M2M_SRC].field = V4L2_FIELD_INTERLACED_TB; 884 885 return vb2_queue_init(dst_vq); 886 } 887 888 /* 889 * File operations 890 */ 891 static int deinterlace_open(struct file *file) 892 { 893 struct deinterlace_dev *pcdev = video_drvdata(file); 894 struct deinterlace_ctx *ctx = NULL; 895 896 ctx = kzalloc(sizeof *ctx, GFP_KERNEL); 897 if (!ctx) 898 return -ENOMEM; 899 900 file->private_data = ctx; 901 ctx->dev = pcdev; 902 903 ctx->m2m_ctx = v4l2_m2m_ctx_init(pcdev->m2m_dev, ctx, &queue_init); 904 if (IS_ERR(ctx->m2m_ctx)) { 905 int ret = PTR_ERR(ctx->m2m_ctx); 906 907 kfree(ctx); 908 return ret; 909 } 910 911 ctx->xt = kzalloc(sizeof(struct dma_interleaved_template) + 912 sizeof(struct data_chunk), GFP_KERNEL); 913 if (!ctx->xt) { 914 kfree(ctx); 915 return -ENOMEM; 916 } 917 918 ctx->colorspace = V4L2_COLORSPACE_REC709; 919 920 dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n", ctx, ctx->m2m_ctx); 921 922 return 0; 923 } 924 925 static int deinterlace_release(struct file *file) 926 { 927 struct deinterlace_dev *pcdev = video_drvdata(file); 928 struct deinterlace_ctx *ctx = file->private_data; 929 930 dprintk(pcdev, "Releasing instance %p\n", ctx); 931 932 v4l2_m2m_ctx_release(ctx->m2m_ctx); 933 kfree(ctx->xt); 934 kfree(ctx); 935 936 return 0; 937 } 938 939 static __poll_t deinterlace_poll(struct file *file, 940 struct poll_table_struct *wait) 941 { 942 struct deinterlace_ctx *ctx = file->private_data; 943 __poll_t ret; 944 945 mutex_lock(&ctx->dev->dev_mutex); 946 ret = v4l2_m2m_poll(file, ctx->m2m_ctx, wait); 947 mutex_unlock(&ctx->dev->dev_mutex); 948 949 return ret; 950 } 951 952 static int deinterlace_mmap(struct file *file, struct vm_area_struct *vma) 953 { 954 struct deinterlace_ctx *ctx = file->private_data; 955 956 return v4l2_m2m_mmap(file, ctx->m2m_ctx, vma); 957 } 958 959 static const struct v4l2_file_operations deinterlace_fops = { 960 .owner = THIS_MODULE, 961 .open = deinterlace_open, 962 .release = deinterlace_release, 963 .poll = deinterlace_poll, 964 .unlocked_ioctl = video_ioctl2, 965 .mmap = deinterlace_mmap, 966 }; 967 968 static const struct video_device deinterlace_videodev = { 969 .name = MEM2MEM_NAME, 970 .fops = &deinterlace_fops, 971 .ioctl_ops = &deinterlace_ioctl_ops, 972 .minor = -1, 973 .release = video_device_release_empty, 974 .vfl_dir = VFL_DIR_M2M, 975 }; 976 977 static const struct v4l2_m2m_ops m2m_ops = { 978 .device_run = deinterlace_device_run, 979 .job_ready = deinterlace_job_ready, 980 .job_abort = deinterlace_job_abort, 981 }; 982 983 static int deinterlace_probe(struct platform_device *pdev) 984 { 985 struct deinterlace_dev *pcdev; 986 struct video_device *vfd; 987 dma_cap_mask_t mask; 988 int ret = 0; 989 990 pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL); 991 if (!pcdev) 992 return -ENOMEM; 993 994 spin_lock_init(&pcdev->irqlock); 995 996 dma_cap_zero(mask); 997 dma_cap_set(DMA_INTERLEAVE, mask); 998 pcdev->dma_chan = dma_request_channel(mask, NULL, pcdev); 999 if (!pcdev->dma_chan) 1000 return -ENODEV; 1001 1002 if (!dma_has_cap(DMA_INTERLEAVE, pcdev->dma_chan->device->cap_mask)) { 1003 dev_err(&pdev->dev, "DMA does not support INTERLEAVE\n"); 1004 ret = -ENODEV; 1005 goto rel_dma; 1006 } 1007 1008 ret = v4l2_device_register(&pdev->dev, &pcdev->v4l2_dev); 1009 if (ret) 1010 goto rel_dma; 1011 1012 atomic_set(&pcdev->busy, 0); 1013 mutex_init(&pcdev->dev_mutex); 1014 1015 vfd = &pcdev->vfd; 1016 *vfd = deinterlace_videodev; 1017 vfd->lock = &pcdev->dev_mutex; 1018 vfd->v4l2_dev = &pcdev->v4l2_dev; 1019 1020 ret = video_register_device(vfd, VFL_TYPE_GRABBER, 0); 1021 if (ret) { 1022 v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n"); 1023 goto unreg_dev; 1024 } 1025 1026 video_set_drvdata(vfd, pcdev); 1027 v4l2_info(&pcdev->v4l2_dev, MEM2MEM_TEST_MODULE_NAME 1028 " Device registered as /dev/video%d\n", vfd->num); 1029 1030 platform_set_drvdata(pdev, pcdev); 1031 1032 pcdev->m2m_dev = v4l2_m2m_init(&m2m_ops); 1033 if (IS_ERR(pcdev->m2m_dev)) { 1034 v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n"); 1035 ret = PTR_ERR(pcdev->m2m_dev); 1036 goto err_m2m; 1037 } 1038 1039 return 0; 1040 1041 err_m2m: 1042 video_unregister_device(&pcdev->vfd); 1043 unreg_dev: 1044 v4l2_device_unregister(&pcdev->v4l2_dev); 1045 rel_dma: 1046 dma_release_channel(pcdev->dma_chan); 1047 1048 return ret; 1049 } 1050 1051 static int deinterlace_remove(struct platform_device *pdev) 1052 { 1053 struct deinterlace_dev *pcdev = platform_get_drvdata(pdev); 1054 1055 v4l2_info(&pcdev->v4l2_dev, "Removing " MEM2MEM_TEST_MODULE_NAME); 1056 v4l2_m2m_release(pcdev->m2m_dev); 1057 video_unregister_device(&pcdev->vfd); 1058 v4l2_device_unregister(&pcdev->v4l2_dev); 1059 dma_release_channel(pcdev->dma_chan); 1060 1061 return 0; 1062 } 1063 1064 static struct platform_driver deinterlace_pdrv = { 1065 .probe = deinterlace_probe, 1066 .remove = deinterlace_remove, 1067 .driver = { 1068 .name = MEM2MEM_NAME, 1069 }, 1070 }; 1071 module_platform_driver(deinterlace_pdrv); 1072 1073