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