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