1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * A virtual v4l2-mem2mem example device. 4 * 5 * This is a virtual device driver for testing mem-to-mem vb2 framework. 6 * It simulates a device that uses memory buffers for both source and 7 * destination, processes the data and issues an "irq" (simulated by a delayed 8 * workqueue). 9 * The device is capable of multi-instance, multi-buffer-per-transaction 10 * operation (via the mem2mem framework). 11 * 12 * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd. 13 * Pawel Osciak, <pawel@osciak.com> 14 * Marek Szyprowski, <m.szyprowski@samsung.com> 15 */ 16 #include <linux/module.h> 17 #include <linux/delay.h> 18 #include <linux/fs.h> 19 #include <linux/sched.h> 20 #include <linux/slab.h> 21 22 #include <linux/platform_device.h> 23 #include <media/v4l2-mem2mem.h> 24 #include <media/v4l2-device.h> 25 #include <media/v4l2-ioctl.h> 26 #include <media/v4l2-ctrls.h> 27 #include <media/v4l2-event.h> 28 #include <media/videobuf2-vmalloc.h> 29 30 MODULE_DESCRIPTION("Virtual device for mem2mem framework testing"); 31 MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>"); 32 MODULE_LICENSE("GPL"); 33 MODULE_VERSION("0.2"); 34 MODULE_ALIAS("mem2mem_testdev"); 35 36 static unsigned int debug; 37 module_param(debug, uint, 0644); 38 MODULE_PARM_DESC(debug, "debug level"); 39 40 /* Default transaction time in msec */ 41 static unsigned int default_transtime = 40; /* Max 25 fps */ 42 module_param(default_transtime, uint, 0644); 43 MODULE_PARM_DESC(default_transtime, "default transaction time in ms"); 44 45 #define MIN_W 32 46 #define MIN_H 32 47 #define MAX_W 640 48 #define MAX_H 480 49 50 /* Pixel alignment for non-bayer formats */ 51 #define WIDTH_ALIGN 2 52 #define HEIGHT_ALIGN 1 53 54 /* Pixel alignment for bayer formats */ 55 #define BAYER_WIDTH_ALIGN 2 56 #define BAYER_HEIGHT_ALIGN 2 57 58 /* Flags that indicate a format can be used for capture/output */ 59 #define MEM2MEM_CAPTURE BIT(0) 60 #define MEM2MEM_OUTPUT BIT(1) 61 62 #define MEM2MEM_NAME "vim2m" 63 64 /* Per queue */ 65 #define MEM2MEM_DEF_NUM_BUFS VIDEO_MAX_FRAME 66 /* In bytes, per queue */ 67 #define MEM2MEM_VID_MEM_LIMIT (16 * 1024 * 1024) 68 69 /* Flags that indicate processing mode */ 70 #define MEM2MEM_HFLIP BIT(0) 71 #define MEM2MEM_VFLIP BIT(1) 72 73 #define dprintk(dev, lvl, fmt, arg...) \ 74 v4l2_dbg(lvl, debug, &(dev)->v4l2_dev, "%s: " fmt, __func__, ## arg) 75 76 static void vim2m_dev_release(struct device *dev) 77 {} 78 79 static struct platform_device vim2m_pdev = { 80 .name = MEM2MEM_NAME, 81 .dev.release = vim2m_dev_release, 82 }; 83 84 struct vim2m_fmt { 85 u32 fourcc; 86 int depth; 87 /* Types the format can be used for */ 88 u32 types; 89 }; 90 91 static struct vim2m_fmt formats[] = { 92 { 93 .fourcc = V4L2_PIX_FMT_RGB565, /* rrrrrggg gggbbbbb */ 94 .depth = 16, 95 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT, 96 }, { 97 .fourcc = V4L2_PIX_FMT_RGB565X, /* gggbbbbb rrrrrggg */ 98 .depth = 16, 99 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT, 100 }, { 101 .fourcc = V4L2_PIX_FMT_RGB24, 102 .depth = 24, 103 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT, 104 }, { 105 .fourcc = V4L2_PIX_FMT_BGR24, 106 .depth = 24, 107 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT, 108 }, { 109 .fourcc = V4L2_PIX_FMT_YUYV, 110 .depth = 16, 111 .types = MEM2MEM_CAPTURE, 112 }, { 113 .fourcc = V4L2_PIX_FMT_SBGGR8, 114 .depth = 8, 115 .types = MEM2MEM_CAPTURE, 116 }, { 117 .fourcc = V4L2_PIX_FMT_SGBRG8, 118 .depth = 8, 119 .types = MEM2MEM_CAPTURE, 120 }, { 121 .fourcc = V4L2_PIX_FMT_SGRBG8, 122 .depth = 8, 123 .types = MEM2MEM_CAPTURE, 124 }, { 125 .fourcc = V4L2_PIX_FMT_SRGGB8, 126 .depth = 8, 127 .types = MEM2MEM_CAPTURE, 128 }, 129 }; 130 131 #define NUM_FORMATS ARRAY_SIZE(formats) 132 133 /* Per-queue, driver-specific private data */ 134 struct vim2m_q_data { 135 unsigned int width; 136 unsigned int height; 137 unsigned int sizeimage; 138 unsigned int sequence; 139 struct vim2m_fmt *fmt; 140 }; 141 142 enum { 143 V4L2_M2M_SRC = 0, 144 V4L2_M2M_DST = 1, 145 }; 146 147 #define V4L2_CID_TRANS_TIME_MSEC (V4L2_CID_USER_BASE + 0x1000) 148 #define V4L2_CID_TRANS_NUM_BUFS (V4L2_CID_USER_BASE + 0x1001) 149 150 static struct vim2m_fmt *find_format(u32 fourcc) 151 { 152 struct vim2m_fmt *fmt; 153 unsigned int k; 154 155 for (k = 0; k < NUM_FORMATS; k++) { 156 fmt = &formats[k]; 157 if (fmt->fourcc == fourcc) 158 break; 159 } 160 161 if (k == NUM_FORMATS) 162 return NULL; 163 164 return &formats[k]; 165 } 166 167 static void get_alignment(u32 fourcc, 168 unsigned int *walign, unsigned int *halign) 169 { 170 switch (fourcc) { 171 case V4L2_PIX_FMT_SBGGR8: 172 case V4L2_PIX_FMT_SGBRG8: 173 case V4L2_PIX_FMT_SGRBG8: 174 case V4L2_PIX_FMT_SRGGB8: 175 *walign = BAYER_WIDTH_ALIGN; 176 *halign = BAYER_HEIGHT_ALIGN; 177 return; 178 default: 179 *walign = WIDTH_ALIGN; 180 *halign = HEIGHT_ALIGN; 181 return; 182 } 183 } 184 185 struct vim2m_dev { 186 struct v4l2_device v4l2_dev; 187 struct video_device vfd; 188 #ifdef CONFIG_MEDIA_CONTROLLER 189 struct media_device mdev; 190 #endif 191 192 atomic_t num_inst; 193 struct mutex dev_mutex; 194 195 struct v4l2_m2m_dev *m2m_dev; 196 }; 197 198 struct vim2m_ctx { 199 struct v4l2_fh fh; 200 struct vim2m_dev *dev; 201 202 struct v4l2_ctrl_handler hdl; 203 204 /* Processed buffers in this transaction */ 205 u8 num_processed; 206 207 /* Transaction length (i.e. how many buffers per transaction) */ 208 u32 translen; 209 /* Transaction time (i.e. simulated processing time) in milliseconds */ 210 u32 transtime; 211 212 struct mutex vb_mutex; 213 struct delayed_work work_run; 214 215 /* Abort requested by m2m */ 216 int aborting; 217 218 /* Processing mode */ 219 int mode; 220 221 enum v4l2_colorspace colorspace; 222 enum v4l2_ycbcr_encoding ycbcr_enc; 223 enum v4l2_xfer_func xfer_func; 224 enum v4l2_quantization quant; 225 226 /* Source and destination queue data */ 227 struct vim2m_q_data q_data[2]; 228 }; 229 230 static inline struct vim2m_ctx *file2ctx(struct file *file) 231 { 232 return container_of(file->private_data, struct vim2m_ctx, fh); 233 } 234 235 static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx, 236 enum v4l2_buf_type type) 237 { 238 switch (type) { 239 case V4L2_BUF_TYPE_VIDEO_OUTPUT: 240 return &ctx->q_data[V4L2_M2M_SRC]; 241 case V4L2_BUF_TYPE_VIDEO_CAPTURE: 242 return &ctx->q_data[V4L2_M2M_DST]; 243 default: 244 return NULL; 245 } 246 } 247 248 static const char *type_name(enum v4l2_buf_type type) 249 { 250 switch (type) { 251 case V4L2_BUF_TYPE_VIDEO_OUTPUT: 252 return "Output"; 253 case V4L2_BUF_TYPE_VIDEO_CAPTURE: 254 return "Capture"; 255 default: 256 return "Invalid"; 257 } 258 } 259 260 #define CLIP(__color) \ 261 (u8)(((__color) > 0xff) ? 0xff : (((__color) < 0) ? 0 : (__color))) 262 263 static void copy_line(struct vim2m_q_data *q_data_out, 264 u8 *src, u8 *dst, bool reverse) 265 { 266 int x, depth = q_data_out->fmt->depth >> 3; 267 268 if (!reverse) { 269 memcpy(dst, src, q_data_out->width * depth); 270 } else { 271 for (x = 0; x < q_data_out->width >> 1; x++) { 272 memcpy(dst, src, depth); 273 memcpy(dst + depth, src - depth, depth); 274 src -= depth << 1; 275 dst += depth << 1; 276 } 277 return; 278 } 279 } 280 281 static void copy_two_pixels(struct vim2m_q_data *q_data_in, 282 struct vim2m_q_data *q_data_out, 283 u8 *src[2], u8 **dst, int ypos, bool reverse) 284 { 285 struct vim2m_fmt *out = q_data_out->fmt; 286 struct vim2m_fmt *in = q_data_in->fmt; 287 u8 _r[2], _g[2], _b[2], *r, *g, *b; 288 int i; 289 290 /* Step 1: read two consecutive pixels from src pointer */ 291 292 r = _r; 293 g = _g; 294 b = _b; 295 296 switch (in->fourcc) { 297 case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */ 298 for (i = 0; i < 2; i++) { 299 u16 pix = le16_to_cpu(*(__le16 *)(src[i])); 300 301 *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07; 302 *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03; 303 *b++ = (u8)((pix & 0x1f) << 3) | 0x07; 304 } 305 break; 306 case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */ 307 for (i = 0; i < 2; i++) { 308 u16 pix = be16_to_cpu(*(__be16 *)(src[i])); 309 310 *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07; 311 *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03; 312 *b++ = (u8)((pix & 0x1f) << 3) | 0x07; 313 } 314 break; 315 default: 316 case V4L2_PIX_FMT_RGB24: 317 for (i = 0; i < 2; i++) { 318 *r++ = src[i][0]; 319 *g++ = src[i][1]; 320 *b++ = src[i][2]; 321 } 322 break; 323 case V4L2_PIX_FMT_BGR24: 324 for (i = 0; i < 2; i++) { 325 *b++ = src[i][0]; 326 *g++ = src[i][1]; 327 *r++ = src[i][2]; 328 } 329 break; 330 } 331 332 /* Step 2: store two consecutive points, reversing them if needed */ 333 334 r = _r; 335 g = _g; 336 b = _b; 337 338 switch (out->fourcc) { 339 case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */ 340 for (i = 0; i < 2; i++) { 341 u16 pix; 342 __le16 *dst_pix = (__le16 *)*dst; 343 344 pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) | 345 (*b >> 3); 346 347 *dst_pix = cpu_to_le16(pix); 348 349 *dst += 2; 350 } 351 return; 352 case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */ 353 for (i = 0; i < 2; i++) { 354 u16 pix; 355 __be16 *dst_pix = (__be16 *)*dst; 356 357 pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) | 358 (*b >> 3); 359 360 *dst_pix = cpu_to_be16(pix); 361 362 *dst += 2; 363 } 364 return; 365 case V4L2_PIX_FMT_RGB24: 366 for (i = 0; i < 2; i++) { 367 *(*dst)++ = *r++; 368 *(*dst)++ = *g++; 369 *(*dst)++ = *b++; 370 } 371 return; 372 case V4L2_PIX_FMT_BGR24: 373 for (i = 0; i < 2; i++) { 374 *(*dst)++ = *b++; 375 *(*dst)++ = *g++; 376 *(*dst)++ = *r++; 377 } 378 return; 379 case V4L2_PIX_FMT_YUYV: 380 default: 381 { 382 u8 y, y1, u, v; 383 384 y = ((8453 * (*r) + 16594 * (*g) + 3223 * (*b) 385 + 524288) >> 15); 386 u = ((-4878 * (*r) - 9578 * (*g) + 14456 * (*b) 387 + 4210688) >> 15); 388 v = ((14456 * (*r++) - 12105 * (*g++) - 2351 * (*b++) 389 + 4210688) >> 15); 390 y1 = ((8453 * (*r) + 16594 * (*g) + 3223 * (*b) 391 + 524288) >> 15); 392 393 *(*dst)++ = y; 394 *(*dst)++ = u; 395 396 *(*dst)++ = y1; 397 *(*dst)++ = v; 398 return; 399 } 400 case V4L2_PIX_FMT_SBGGR8: 401 if (!(ypos & 1)) { 402 *(*dst)++ = *b; 403 *(*dst)++ = *++g; 404 } else { 405 *(*dst)++ = *g; 406 *(*dst)++ = *++r; 407 } 408 return; 409 case V4L2_PIX_FMT_SGBRG8: 410 if (!(ypos & 1)) { 411 *(*dst)++ = *g; 412 *(*dst)++ = *++b; 413 } else { 414 *(*dst)++ = *r; 415 *(*dst)++ = *++g; 416 } 417 return; 418 case V4L2_PIX_FMT_SGRBG8: 419 if (!(ypos & 1)) { 420 *(*dst)++ = *g; 421 *(*dst)++ = *++r; 422 } else { 423 *(*dst)++ = *b; 424 *(*dst)++ = *++g; 425 } 426 return; 427 case V4L2_PIX_FMT_SRGGB8: 428 if (!(ypos & 1)) { 429 *(*dst)++ = *r; 430 *(*dst)++ = *++g; 431 } else { 432 *(*dst)++ = *g; 433 *(*dst)++ = *++b; 434 } 435 return; 436 } 437 } 438 439 static int device_process(struct vim2m_ctx *ctx, 440 struct vb2_v4l2_buffer *in_vb, 441 struct vb2_v4l2_buffer *out_vb) 442 { 443 struct vim2m_dev *dev = ctx->dev; 444 struct vim2m_q_data *q_data_in, *q_data_out; 445 u8 *p_in, *p_line, *p_in_x[2], *p, *p_out; 446 unsigned int width, height, bytesperline, bytes_per_pixel; 447 unsigned int x, y, y_in, y_out, x_int, x_fract, x_err, x_offset; 448 int start, end, step; 449 450 q_data_in = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT); 451 if (!q_data_in) 452 return 0; 453 bytesperline = (q_data_in->width * q_data_in->fmt->depth) >> 3; 454 bytes_per_pixel = q_data_in->fmt->depth >> 3; 455 456 q_data_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE); 457 if (!q_data_out) 458 return 0; 459 460 /* As we're doing scaling, use the output dimensions here */ 461 height = q_data_out->height; 462 width = q_data_out->width; 463 464 p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0); 465 p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0); 466 if (!p_in || !p_out) { 467 v4l2_err(&dev->v4l2_dev, 468 "Acquiring kernel pointers to buffers failed\n"); 469 return -EFAULT; 470 } 471 472 out_vb->sequence = q_data_out->sequence++; 473 in_vb->sequence = q_data_in->sequence++; 474 v4l2_m2m_buf_copy_metadata(in_vb, out_vb, true); 475 476 if (ctx->mode & MEM2MEM_VFLIP) { 477 start = height - 1; 478 end = -1; 479 step = -1; 480 } else { 481 start = 0; 482 end = height; 483 step = 1; 484 } 485 y_out = 0; 486 487 /* 488 * When format and resolution are identical, 489 * we can use a faster copy logic 490 */ 491 if (q_data_in->fmt->fourcc == q_data_out->fmt->fourcc && 492 q_data_in->width == q_data_out->width && 493 q_data_in->height == q_data_out->height) { 494 for (y = start; y != end; y += step, y_out++) { 495 p = p_in + (y * bytesperline); 496 if (ctx->mode & MEM2MEM_HFLIP) 497 p += bytesperline - (q_data_in->fmt->depth >> 3); 498 499 copy_line(q_data_out, p, p_out, 500 ctx->mode & MEM2MEM_HFLIP); 501 502 p_out += bytesperline; 503 } 504 return 0; 505 } 506 507 /* Slower algorithm with format conversion, hflip, vflip and scaler */ 508 509 /* To speed scaler up, use Bresenham for X dimension */ 510 x_int = q_data_in->width / q_data_out->width; 511 x_fract = q_data_in->width % q_data_out->width; 512 513 for (y = start; y != end; y += step, y_out++) { 514 y_in = (y * q_data_in->height) / q_data_out->height; 515 x_offset = 0; 516 x_err = 0; 517 518 p_line = p_in + (y_in * bytesperline); 519 if (ctx->mode & MEM2MEM_HFLIP) 520 p_line += bytesperline - (q_data_in->fmt->depth >> 3); 521 p_in_x[0] = p_line; 522 523 for (x = 0; x < width >> 1; x++) { 524 x_offset += x_int; 525 x_err += x_fract; 526 if (x_err > width) { 527 x_offset++; 528 x_err -= width; 529 } 530 531 if (ctx->mode & MEM2MEM_HFLIP) 532 p_in_x[1] = p_line - x_offset * bytes_per_pixel; 533 else 534 p_in_x[1] = p_line + x_offset * bytes_per_pixel; 535 536 copy_two_pixels(q_data_in, q_data_out, 537 p_in_x, &p_out, y_out, 538 ctx->mode & MEM2MEM_HFLIP); 539 540 /* Calculate the next p_in_x0 */ 541 x_offset += x_int; 542 x_err += x_fract; 543 if (x_err > width) { 544 x_offset++; 545 x_err -= width; 546 } 547 548 if (ctx->mode & MEM2MEM_HFLIP) 549 p_in_x[0] = p_line - x_offset * bytes_per_pixel; 550 else 551 p_in_x[0] = p_line + x_offset * bytes_per_pixel; 552 } 553 } 554 555 return 0; 556 } 557 558 /* 559 * mem2mem callbacks 560 */ 561 562 /* 563 * job_ready() - check whether an instance is ready to be scheduled to run 564 */ 565 static int job_ready(void *priv) 566 { 567 struct vim2m_ctx *ctx = priv; 568 569 if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen 570 || v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen) { 571 dprintk(ctx->dev, 1, "Not enough buffers available\n"); 572 return 0; 573 } 574 575 return 1; 576 } 577 578 static void job_abort(void *priv) 579 { 580 struct vim2m_ctx *ctx = priv; 581 582 /* Will cancel the transaction in the next interrupt handler */ 583 ctx->aborting = 1; 584 } 585 586 /* device_run() - prepares and starts the device 587 * 588 * This simulates all the immediate preparations required before starting 589 * a device. This will be called by the framework when it decides to schedule 590 * a particular instance. 591 */ 592 static void device_run(void *priv) 593 { 594 struct vim2m_ctx *ctx = priv; 595 struct vb2_v4l2_buffer *src_buf, *dst_buf; 596 597 src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx); 598 dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx); 599 600 /* Apply request controls if any */ 601 v4l2_ctrl_request_setup(src_buf->vb2_buf.req_obj.req, 602 &ctx->hdl); 603 604 device_process(ctx, src_buf, dst_buf); 605 606 /* Complete request controls if any */ 607 v4l2_ctrl_request_complete(src_buf->vb2_buf.req_obj.req, 608 &ctx->hdl); 609 610 /* Run delayed work, which simulates a hardware irq */ 611 schedule_delayed_work(&ctx->work_run, msecs_to_jiffies(ctx->transtime)); 612 } 613 614 static void device_work(struct work_struct *w) 615 { 616 struct vim2m_ctx *curr_ctx; 617 struct vim2m_dev *vim2m_dev; 618 struct vb2_v4l2_buffer *src_vb, *dst_vb; 619 620 curr_ctx = container_of(w, struct vim2m_ctx, work_run.work); 621 622 vim2m_dev = curr_ctx->dev; 623 624 src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx); 625 dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx); 626 627 curr_ctx->num_processed++; 628 629 v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE); 630 v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE); 631 632 if (curr_ctx->num_processed == curr_ctx->translen 633 || curr_ctx->aborting) { 634 dprintk(curr_ctx->dev, 2, "Finishing capture buffer fill\n"); 635 curr_ctx->num_processed = 0; 636 v4l2_m2m_job_finish(vim2m_dev->m2m_dev, curr_ctx->fh.m2m_ctx); 637 } else { 638 device_run(curr_ctx); 639 } 640 } 641 642 /* 643 * video ioctls 644 */ 645 static int vidioc_querycap(struct file *file, void *priv, 646 struct v4l2_capability *cap) 647 { 648 strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver)); 649 strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card)); 650 snprintf(cap->bus_info, sizeof(cap->bus_info), 651 "platform:%s", MEM2MEM_NAME); 652 return 0; 653 } 654 655 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type) 656 { 657 int i, num; 658 struct vim2m_fmt *fmt; 659 660 num = 0; 661 662 for (i = 0; i < NUM_FORMATS; ++i) { 663 if (formats[i].types & type) { 664 /* index-th format of type type found ? */ 665 if (num == f->index) 666 break; 667 /* 668 * Correct type but haven't reached our index yet, 669 * just increment per-type index 670 */ 671 ++num; 672 } 673 } 674 675 if (i < NUM_FORMATS) { 676 /* Format found */ 677 fmt = &formats[i]; 678 f->pixelformat = fmt->fourcc; 679 return 0; 680 } 681 682 /* Format not found */ 683 return -EINVAL; 684 } 685 686 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv, 687 struct v4l2_fmtdesc *f) 688 { 689 return enum_fmt(f, MEM2MEM_CAPTURE); 690 } 691 692 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv, 693 struct v4l2_fmtdesc *f) 694 { 695 return enum_fmt(f, MEM2MEM_OUTPUT); 696 } 697 698 static int vidioc_enum_framesizes(struct file *file, void *priv, 699 struct v4l2_frmsizeenum *fsize) 700 { 701 if (fsize->index != 0) 702 return -EINVAL; 703 704 if (!find_format(fsize->pixel_format)) 705 return -EINVAL; 706 707 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; 708 fsize->stepwise.min_width = MIN_W; 709 fsize->stepwise.min_height = MIN_H; 710 fsize->stepwise.max_width = MAX_W; 711 fsize->stepwise.max_height = MAX_H; 712 713 get_alignment(fsize->pixel_format, 714 &fsize->stepwise.step_width, 715 &fsize->stepwise.step_height); 716 return 0; 717 } 718 719 static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f) 720 { 721 struct vb2_queue *vq; 722 struct vim2m_q_data *q_data; 723 724 vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type); 725 if (!vq) 726 return -EINVAL; 727 728 q_data = get_q_data(ctx, f->type); 729 if (!q_data) 730 return -EINVAL; 731 732 f->fmt.pix.width = q_data->width; 733 f->fmt.pix.height = q_data->height; 734 f->fmt.pix.field = V4L2_FIELD_NONE; 735 f->fmt.pix.pixelformat = q_data->fmt->fourcc; 736 f->fmt.pix.bytesperline = (q_data->width * q_data->fmt->depth) >> 3; 737 f->fmt.pix.sizeimage = q_data->sizeimage; 738 f->fmt.pix.colorspace = ctx->colorspace; 739 f->fmt.pix.xfer_func = ctx->xfer_func; 740 f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc; 741 f->fmt.pix.quantization = ctx->quant; 742 743 return 0; 744 } 745 746 static int vidioc_g_fmt_vid_out(struct file *file, void *priv, 747 struct v4l2_format *f) 748 { 749 return vidioc_g_fmt(file2ctx(file), f); 750 } 751 752 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv, 753 struct v4l2_format *f) 754 { 755 return vidioc_g_fmt(file2ctx(file), f); 756 } 757 758 static int vidioc_try_fmt(struct v4l2_format *f, struct vim2m_fmt *fmt) 759 { 760 int walign, halign; 761 /* 762 * V4L2 specification specifies the driver corrects the 763 * format struct if any of the dimensions is unsupported 764 */ 765 if (f->fmt.pix.height < MIN_H) 766 f->fmt.pix.height = MIN_H; 767 else if (f->fmt.pix.height > MAX_H) 768 f->fmt.pix.height = MAX_H; 769 770 if (f->fmt.pix.width < MIN_W) 771 f->fmt.pix.width = MIN_W; 772 else if (f->fmt.pix.width > MAX_W) 773 f->fmt.pix.width = MAX_W; 774 775 get_alignment(f->fmt.pix.pixelformat, &walign, &halign); 776 f->fmt.pix.width &= ~(walign - 1); 777 f->fmt.pix.height &= ~(halign - 1); 778 f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3; 779 f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline; 780 f->fmt.pix.field = V4L2_FIELD_NONE; 781 782 return 0; 783 } 784 785 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv, 786 struct v4l2_format *f) 787 { 788 struct vim2m_fmt *fmt; 789 struct vim2m_ctx *ctx = file2ctx(file); 790 791 fmt = find_format(f->fmt.pix.pixelformat); 792 if (!fmt) { 793 f->fmt.pix.pixelformat = formats[0].fourcc; 794 fmt = find_format(f->fmt.pix.pixelformat); 795 } 796 if (!(fmt->types & MEM2MEM_CAPTURE)) { 797 v4l2_err(&ctx->dev->v4l2_dev, 798 "Fourcc format (0x%08x) invalid.\n", 799 f->fmt.pix.pixelformat); 800 return -EINVAL; 801 } 802 f->fmt.pix.colorspace = ctx->colorspace; 803 f->fmt.pix.xfer_func = ctx->xfer_func; 804 f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc; 805 f->fmt.pix.quantization = ctx->quant; 806 807 return vidioc_try_fmt(f, fmt); 808 } 809 810 static int vidioc_try_fmt_vid_out(struct file *file, void *priv, 811 struct v4l2_format *f) 812 { 813 struct vim2m_fmt *fmt; 814 struct vim2m_ctx *ctx = file2ctx(file); 815 816 fmt = find_format(f->fmt.pix.pixelformat); 817 if (!fmt) { 818 f->fmt.pix.pixelformat = formats[0].fourcc; 819 fmt = find_format(f->fmt.pix.pixelformat); 820 } 821 if (!(fmt->types & MEM2MEM_OUTPUT)) { 822 v4l2_err(&ctx->dev->v4l2_dev, 823 "Fourcc format (0x%08x) invalid.\n", 824 f->fmt.pix.pixelformat); 825 return -EINVAL; 826 } 827 if (!f->fmt.pix.colorspace) 828 f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709; 829 830 return vidioc_try_fmt(f, fmt); 831 } 832 833 static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f) 834 { 835 struct vim2m_q_data *q_data; 836 struct vb2_queue *vq; 837 838 vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type); 839 if (!vq) 840 return -EINVAL; 841 842 q_data = get_q_data(ctx, f->type); 843 if (!q_data) 844 return -EINVAL; 845 846 if (vb2_is_busy(vq)) { 847 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__); 848 return -EBUSY; 849 } 850 851 q_data->fmt = find_format(f->fmt.pix.pixelformat); 852 q_data->width = f->fmt.pix.width; 853 q_data->height = f->fmt.pix.height; 854 q_data->sizeimage = q_data->width * q_data->height 855 * q_data->fmt->depth >> 3; 856 857 dprintk(ctx->dev, 1, 858 "Format for type %s: %dx%d (%d bpp), fmt: %c%c%c%c\n", 859 type_name(f->type), q_data->width, q_data->height, 860 q_data->fmt->depth, 861 (q_data->fmt->fourcc & 0xff), 862 (q_data->fmt->fourcc >> 8) & 0xff, 863 (q_data->fmt->fourcc >> 16) & 0xff, 864 (q_data->fmt->fourcc >> 24) & 0xff); 865 866 return 0; 867 } 868 869 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv, 870 struct v4l2_format *f) 871 { 872 int ret; 873 874 ret = vidioc_try_fmt_vid_cap(file, priv, f); 875 if (ret) 876 return ret; 877 878 return vidioc_s_fmt(file2ctx(file), f); 879 } 880 881 static int vidioc_s_fmt_vid_out(struct file *file, void *priv, 882 struct v4l2_format *f) 883 { 884 struct vim2m_ctx *ctx = file2ctx(file); 885 int ret; 886 887 ret = vidioc_try_fmt_vid_out(file, priv, f); 888 if (ret) 889 return ret; 890 891 ret = vidioc_s_fmt(file2ctx(file), f); 892 if (!ret) { 893 ctx->colorspace = f->fmt.pix.colorspace; 894 ctx->xfer_func = f->fmt.pix.xfer_func; 895 ctx->ycbcr_enc = f->fmt.pix.ycbcr_enc; 896 ctx->quant = f->fmt.pix.quantization; 897 } 898 return ret; 899 } 900 901 static int vim2m_s_ctrl(struct v4l2_ctrl *ctrl) 902 { 903 struct vim2m_ctx *ctx = 904 container_of(ctrl->handler, struct vim2m_ctx, hdl); 905 906 switch (ctrl->id) { 907 case V4L2_CID_HFLIP: 908 if (ctrl->val) 909 ctx->mode |= MEM2MEM_HFLIP; 910 else 911 ctx->mode &= ~MEM2MEM_HFLIP; 912 break; 913 914 case V4L2_CID_VFLIP: 915 if (ctrl->val) 916 ctx->mode |= MEM2MEM_VFLIP; 917 else 918 ctx->mode &= ~MEM2MEM_VFLIP; 919 break; 920 921 case V4L2_CID_TRANS_TIME_MSEC: 922 ctx->transtime = ctrl->val; 923 if (ctx->transtime < 1) 924 ctx->transtime = 1; 925 break; 926 927 case V4L2_CID_TRANS_NUM_BUFS: 928 ctx->translen = ctrl->val; 929 break; 930 931 default: 932 v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n"); 933 return -EINVAL; 934 } 935 936 return 0; 937 } 938 939 static const struct v4l2_ctrl_ops vim2m_ctrl_ops = { 940 .s_ctrl = vim2m_s_ctrl, 941 }; 942 943 static const struct v4l2_ioctl_ops vim2m_ioctl_ops = { 944 .vidioc_querycap = vidioc_querycap, 945 946 .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap, 947 .vidioc_enum_framesizes = vidioc_enum_framesizes, 948 .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap, 949 .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap, 950 .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap, 951 952 .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out, 953 .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out, 954 .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out, 955 .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out, 956 957 .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs, 958 .vidioc_querybuf = v4l2_m2m_ioctl_querybuf, 959 .vidioc_qbuf = v4l2_m2m_ioctl_qbuf, 960 .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf, 961 .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf, 962 .vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs, 963 .vidioc_expbuf = v4l2_m2m_ioctl_expbuf, 964 965 .vidioc_streamon = v4l2_m2m_ioctl_streamon, 966 .vidioc_streamoff = v4l2_m2m_ioctl_streamoff, 967 968 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, 969 .vidioc_unsubscribe_event = v4l2_event_unsubscribe, 970 }; 971 972 /* 973 * Queue operations 974 */ 975 976 static int vim2m_queue_setup(struct vb2_queue *vq, 977 unsigned int *nbuffers, 978 unsigned int *nplanes, 979 unsigned int sizes[], 980 struct device *alloc_devs[]) 981 { 982 struct vim2m_ctx *ctx = vb2_get_drv_priv(vq); 983 struct vim2m_q_data *q_data; 984 unsigned int size, count = *nbuffers; 985 986 q_data = get_q_data(ctx, vq->type); 987 if (!q_data) 988 return -EINVAL; 989 990 size = q_data->width * q_data->height * q_data->fmt->depth >> 3; 991 992 while (size * count > MEM2MEM_VID_MEM_LIMIT) 993 (count)--; 994 *nbuffers = count; 995 996 if (*nplanes) 997 return sizes[0] < size ? -EINVAL : 0; 998 999 *nplanes = 1; 1000 sizes[0] = size; 1001 1002 dprintk(ctx->dev, 1, "%s: get %d buffer(s) of size %d each.\n", 1003 type_name(vq->type), count, size); 1004 1005 return 0; 1006 } 1007 1008 static int vim2m_buf_out_validate(struct vb2_buffer *vb) 1009 { 1010 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 1011 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 1012 1013 if (vbuf->field == V4L2_FIELD_ANY) 1014 vbuf->field = V4L2_FIELD_NONE; 1015 if (vbuf->field != V4L2_FIELD_NONE) { 1016 dprintk(ctx->dev, 1, "%s field isn't supported\n", __func__); 1017 return -EINVAL; 1018 } 1019 1020 return 0; 1021 } 1022 1023 static int vim2m_buf_prepare(struct vb2_buffer *vb) 1024 { 1025 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 1026 struct vim2m_q_data *q_data; 1027 1028 dprintk(ctx->dev, 2, "type: %s\n", type_name(vb->vb2_queue->type)); 1029 1030 q_data = get_q_data(ctx, vb->vb2_queue->type); 1031 if (!q_data) 1032 return -EINVAL; 1033 if (vb2_plane_size(vb, 0) < q_data->sizeimage) { 1034 dprintk(ctx->dev, 1, 1035 "%s data will not fit into plane (%lu < %lu)\n", 1036 __func__, vb2_plane_size(vb, 0), 1037 (long)q_data->sizeimage); 1038 return -EINVAL; 1039 } 1040 1041 vb2_set_plane_payload(vb, 0, q_data->sizeimage); 1042 1043 return 0; 1044 } 1045 1046 static void vim2m_buf_queue(struct vb2_buffer *vb) 1047 { 1048 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 1049 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 1050 1051 v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf); 1052 } 1053 1054 static int vim2m_start_streaming(struct vb2_queue *q, unsigned int count) 1055 { 1056 struct vim2m_ctx *ctx = vb2_get_drv_priv(q); 1057 struct vim2m_q_data *q_data = get_q_data(ctx, q->type); 1058 1059 if (!q_data) 1060 return -EINVAL; 1061 1062 if (V4L2_TYPE_IS_OUTPUT(q->type)) 1063 ctx->aborting = 0; 1064 1065 q_data->sequence = 0; 1066 return 0; 1067 } 1068 1069 static void vim2m_stop_streaming(struct vb2_queue *q) 1070 { 1071 struct vim2m_ctx *ctx = vb2_get_drv_priv(q); 1072 struct vb2_v4l2_buffer *vbuf; 1073 1074 cancel_delayed_work_sync(&ctx->work_run); 1075 1076 for (;;) { 1077 if (V4L2_TYPE_IS_OUTPUT(q->type)) 1078 vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx); 1079 else 1080 vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx); 1081 if (!vbuf) 1082 return; 1083 v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req, 1084 &ctx->hdl); 1085 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR); 1086 } 1087 } 1088 1089 static void vim2m_buf_request_complete(struct vb2_buffer *vb) 1090 { 1091 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 1092 1093 v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl); 1094 } 1095 1096 static const struct vb2_ops vim2m_qops = { 1097 .queue_setup = vim2m_queue_setup, 1098 .buf_out_validate = vim2m_buf_out_validate, 1099 .buf_prepare = vim2m_buf_prepare, 1100 .buf_queue = vim2m_buf_queue, 1101 .start_streaming = vim2m_start_streaming, 1102 .stop_streaming = vim2m_stop_streaming, 1103 .wait_prepare = vb2_ops_wait_prepare, 1104 .wait_finish = vb2_ops_wait_finish, 1105 .buf_request_complete = vim2m_buf_request_complete, 1106 }; 1107 1108 static int queue_init(void *priv, struct vb2_queue *src_vq, 1109 struct vb2_queue *dst_vq) 1110 { 1111 struct vim2m_ctx *ctx = priv; 1112 int ret; 1113 1114 src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT; 1115 src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; 1116 src_vq->drv_priv = ctx; 1117 src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); 1118 src_vq->ops = &vim2m_qops; 1119 src_vq->mem_ops = &vb2_vmalloc_memops; 1120 src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; 1121 src_vq->lock = &ctx->vb_mutex; 1122 src_vq->supports_requests = true; 1123 1124 ret = vb2_queue_init(src_vq); 1125 if (ret) 1126 return ret; 1127 1128 dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 1129 dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; 1130 dst_vq->drv_priv = ctx; 1131 dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); 1132 dst_vq->ops = &vim2m_qops; 1133 dst_vq->mem_ops = &vb2_vmalloc_memops; 1134 dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; 1135 dst_vq->lock = &ctx->vb_mutex; 1136 1137 return vb2_queue_init(dst_vq); 1138 } 1139 1140 static struct v4l2_ctrl_config vim2m_ctrl_trans_time_msec = { 1141 .ops = &vim2m_ctrl_ops, 1142 .id = V4L2_CID_TRANS_TIME_MSEC, 1143 .name = "Transaction Time (msec)", 1144 .type = V4L2_CTRL_TYPE_INTEGER, 1145 .min = 1, 1146 .max = 10001, 1147 .step = 1, 1148 }; 1149 1150 static const struct v4l2_ctrl_config vim2m_ctrl_trans_num_bufs = { 1151 .ops = &vim2m_ctrl_ops, 1152 .id = V4L2_CID_TRANS_NUM_BUFS, 1153 .name = "Buffers Per Transaction", 1154 .type = V4L2_CTRL_TYPE_INTEGER, 1155 .def = 1, 1156 .min = 1, 1157 .max = MEM2MEM_DEF_NUM_BUFS, 1158 .step = 1, 1159 }; 1160 1161 /* 1162 * File operations 1163 */ 1164 static int vim2m_open(struct file *file) 1165 { 1166 struct vim2m_dev *dev = video_drvdata(file); 1167 struct vim2m_ctx *ctx = NULL; 1168 struct v4l2_ctrl_handler *hdl; 1169 int rc = 0; 1170 1171 if (mutex_lock_interruptible(&dev->dev_mutex)) 1172 return -ERESTARTSYS; 1173 ctx = kzalloc(sizeof(*ctx), GFP_KERNEL); 1174 if (!ctx) { 1175 rc = -ENOMEM; 1176 goto open_unlock; 1177 } 1178 1179 v4l2_fh_init(&ctx->fh, video_devdata(file)); 1180 file->private_data = &ctx->fh; 1181 ctx->dev = dev; 1182 hdl = &ctx->hdl; 1183 v4l2_ctrl_handler_init(hdl, 4); 1184 v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0); 1185 v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0); 1186 1187 vim2m_ctrl_trans_time_msec.def = default_transtime; 1188 v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_time_msec, NULL); 1189 v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_num_bufs, NULL); 1190 if (hdl->error) { 1191 rc = hdl->error; 1192 v4l2_ctrl_handler_free(hdl); 1193 kfree(ctx); 1194 goto open_unlock; 1195 } 1196 ctx->fh.ctrl_handler = hdl; 1197 v4l2_ctrl_handler_setup(hdl); 1198 1199 ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0]; 1200 ctx->q_data[V4L2_M2M_SRC].width = 640; 1201 ctx->q_data[V4L2_M2M_SRC].height = 480; 1202 ctx->q_data[V4L2_M2M_SRC].sizeimage = 1203 ctx->q_data[V4L2_M2M_SRC].width * 1204 ctx->q_data[V4L2_M2M_SRC].height * 1205 (ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3); 1206 ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC]; 1207 ctx->colorspace = V4L2_COLORSPACE_REC709; 1208 1209 ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init); 1210 1211 mutex_init(&ctx->vb_mutex); 1212 INIT_DELAYED_WORK(&ctx->work_run, device_work); 1213 1214 if (IS_ERR(ctx->fh.m2m_ctx)) { 1215 rc = PTR_ERR(ctx->fh.m2m_ctx); 1216 1217 v4l2_ctrl_handler_free(hdl); 1218 v4l2_fh_exit(&ctx->fh); 1219 kfree(ctx); 1220 goto open_unlock; 1221 } 1222 1223 v4l2_fh_add(&ctx->fh); 1224 atomic_inc(&dev->num_inst); 1225 1226 dprintk(dev, 1, "Created instance: %p, m2m_ctx: %p\n", 1227 ctx, ctx->fh.m2m_ctx); 1228 1229 open_unlock: 1230 mutex_unlock(&dev->dev_mutex); 1231 return rc; 1232 } 1233 1234 static int vim2m_release(struct file *file) 1235 { 1236 struct vim2m_dev *dev = video_drvdata(file); 1237 struct vim2m_ctx *ctx = file2ctx(file); 1238 1239 dprintk(dev, 1, "Releasing instance %p\n", ctx); 1240 1241 v4l2_fh_del(&ctx->fh); 1242 v4l2_fh_exit(&ctx->fh); 1243 v4l2_ctrl_handler_free(&ctx->hdl); 1244 mutex_lock(&dev->dev_mutex); 1245 v4l2_m2m_ctx_release(ctx->fh.m2m_ctx); 1246 mutex_unlock(&dev->dev_mutex); 1247 kfree(ctx); 1248 1249 atomic_dec(&dev->num_inst); 1250 1251 return 0; 1252 } 1253 1254 static void vim2m_device_release(struct video_device *vdev) 1255 { 1256 struct vim2m_dev *dev = container_of(vdev, struct vim2m_dev, vfd); 1257 1258 v4l2_device_unregister(&dev->v4l2_dev); 1259 v4l2_m2m_release(dev->m2m_dev); 1260 #ifdef CONFIG_MEDIA_CONTROLLER 1261 media_device_cleanup(&dev->mdev); 1262 #endif 1263 kfree(dev); 1264 } 1265 1266 static const struct v4l2_file_operations vim2m_fops = { 1267 .owner = THIS_MODULE, 1268 .open = vim2m_open, 1269 .release = vim2m_release, 1270 .poll = v4l2_m2m_fop_poll, 1271 .unlocked_ioctl = video_ioctl2, 1272 .mmap = v4l2_m2m_fop_mmap, 1273 }; 1274 1275 static const struct video_device vim2m_videodev = { 1276 .name = MEM2MEM_NAME, 1277 .vfl_dir = VFL_DIR_M2M, 1278 .fops = &vim2m_fops, 1279 .ioctl_ops = &vim2m_ioctl_ops, 1280 .minor = -1, 1281 .release = vim2m_device_release, 1282 .device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING, 1283 }; 1284 1285 static const struct v4l2_m2m_ops m2m_ops = { 1286 .device_run = device_run, 1287 .job_ready = job_ready, 1288 .job_abort = job_abort, 1289 }; 1290 1291 static const struct media_device_ops m2m_media_ops = { 1292 .req_validate = vb2_request_validate, 1293 .req_queue = v4l2_m2m_request_queue, 1294 }; 1295 1296 static int vim2m_probe(struct platform_device *pdev) 1297 { 1298 struct vim2m_dev *dev; 1299 struct video_device *vfd; 1300 int ret; 1301 1302 dev = kzalloc(sizeof(*dev), GFP_KERNEL); 1303 if (!dev) 1304 return -ENOMEM; 1305 1306 ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev); 1307 if (ret) 1308 goto error_free; 1309 1310 atomic_set(&dev->num_inst, 0); 1311 mutex_init(&dev->dev_mutex); 1312 1313 dev->vfd = vim2m_videodev; 1314 vfd = &dev->vfd; 1315 vfd->lock = &dev->dev_mutex; 1316 vfd->v4l2_dev = &dev->v4l2_dev; 1317 1318 video_set_drvdata(vfd, dev); 1319 v4l2_info(&dev->v4l2_dev, 1320 "Device registered as /dev/video%d\n", vfd->num); 1321 1322 platform_set_drvdata(pdev, dev); 1323 1324 dev->m2m_dev = v4l2_m2m_init(&m2m_ops); 1325 if (IS_ERR(dev->m2m_dev)) { 1326 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n"); 1327 ret = PTR_ERR(dev->m2m_dev); 1328 dev->m2m_dev = NULL; 1329 goto error_dev; 1330 } 1331 1332 #ifdef CONFIG_MEDIA_CONTROLLER 1333 dev->mdev.dev = &pdev->dev; 1334 strscpy(dev->mdev.model, "vim2m", sizeof(dev->mdev.model)); 1335 strscpy(dev->mdev.bus_info, "platform:vim2m", 1336 sizeof(dev->mdev.bus_info)); 1337 media_device_init(&dev->mdev); 1338 dev->mdev.ops = &m2m_media_ops; 1339 dev->v4l2_dev.mdev = &dev->mdev; 1340 #endif 1341 1342 ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0); 1343 if (ret) { 1344 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n"); 1345 goto error_m2m; 1346 } 1347 1348 #ifdef CONFIG_MEDIA_CONTROLLER 1349 ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd, 1350 MEDIA_ENT_F_PROC_VIDEO_SCALER); 1351 if (ret) { 1352 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n"); 1353 goto error_v4l2; 1354 } 1355 1356 ret = media_device_register(&dev->mdev); 1357 if (ret) { 1358 v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n"); 1359 goto error_m2m_mc; 1360 } 1361 #endif 1362 return 0; 1363 1364 #ifdef CONFIG_MEDIA_CONTROLLER 1365 error_m2m_mc: 1366 v4l2_m2m_unregister_media_controller(dev->m2m_dev); 1367 #endif 1368 error_v4l2: 1369 video_unregister_device(&dev->vfd); 1370 /* vim2m_device_release called by video_unregister_device to release various objects */ 1371 return ret; 1372 error_m2m: 1373 v4l2_m2m_release(dev->m2m_dev); 1374 error_dev: 1375 v4l2_device_unregister(&dev->v4l2_dev); 1376 error_free: 1377 kfree(dev); 1378 1379 return ret; 1380 } 1381 1382 static int vim2m_remove(struct platform_device *pdev) 1383 { 1384 struct vim2m_dev *dev = platform_get_drvdata(pdev); 1385 1386 v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME); 1387 1388 #ifdef CONFIG_MEDIA_CONTROLLER 1389 media_device_unregister(&dev->mdev); 1390 v4l2_m2m_unregister_media_controller(dev->m2m_dev); 1391 #endif 1392 video_unregister_device(&dev->vfd); 1393 1394 return 0; 1395 } 1396 1397 static struct platform_driver vim2m_pdrv = { 1398 .probe = vim2m_probe, 1399 .remove = vim2m_remove, 1400 .driver = { 1401 .name = MEM2MEM_NAME, 1402 }, 1403 }; 1404 1405 static void __exit vim2m_exit(void) 1406 { 1407 platform_driver_unregister(&vim2m_pdrv); 1408 platform_device_unregister(&vim2m_pdev); 1409 } 1410 1411 static int __init vim2m_init(void) 1412 { 1413 int ret; 1414 1415 ret = platform_device_register(&vim2m_pdev); 1416 if (ret) 1417 return ret; 1418 1419 ret = platform_driver_register(&vim2m_pdrv); 1420 if (ret) 1421 platform_device_unregister(&vim2m_pdev); 1422 1423 return ret; 1424 } 1425 1426 module_init(vim2m_init); 1427 module_exit(vim2m_exit); 1428