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