1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * TI Camera Access Layer (CAL) - Video Device 4 * 5 * Copyright (c) 2015-2020 Texas Instruments Inc. 6 * 7 * Authors: 8 * Benoit Parrot <bparrot@ti.com> 9 * Laurent Pinchart <laurent.pinchart@ideasonboard.com> 10 */ 11 12 #include <linux/ioctl.h> 13 #include <linux/pm_runtime.h> 14 #include <linux/videodev2.h> 15 16 #include <media/media-device.h> 17 #include <media/v4l2-common.h> 18 #include <media/v4l2-ctrls.h> 19 #include <media/v4l2-device.h> 20 #include <media/v4l2-event.h> 21 #include <media/v4l2-fh.h> 22 #include <media/v4l2-ioctl.h> 23 #include <media/videobuf2-core.h> 24 #include <media/videobuf2-dma-contig.h> 25 26 #include "cal.h" 27 28 /* Print Four-character-code (FOURCC) */ 29 static char *fourcc_to_str(u32 fmt) 30 { 31 static char code[5]; 32 33 code[0] = (unsigned char)(fmt & 0xff); 34 code[1] = (unsigned char)((fmt >> 8) & 0xff); 35 code[2] = (unsigned char)((fmt >> 16) & 0xff); 36 code[3] = (unsigned char)((fmt >> 24) & 0xff); 37 code[4] = '\0'; 38 39 return code; 40 } 41 42 /* ------------------------------------------------------------------ 43 * V4L2 Common IOCTLs 44 * ------------------------------------------------------------------ 45 */ 46 47 static int cal_querycap(struct file *file, void *priv, 48 struct v4l2_capability *cap) 49 { 50 strscpy(cap->driver, CAL_MODULE_NAME, sizeof(cap->driver)); 51 strscpy(cap->card, CAL_MODULE_NAME, sizeof(cap->card)); 52 53 return 0; 54 } 55 56 static int cal_g_fmt_vid_cap(struct file *file, void *priv, 57 struct v4l2_format *f) 58 { 59 struct cal_ctx *ctx = video_drvdata(file); 60 61 *f = ctx->v_fmt; 62 63 return 0; 64 } 65 66 /* ------------------------------------------------------------------ 67 * V4L2 Video Node Centric IOCTLs 68 * ------------------------------------------------------------------ 69 */ 70 71 static const struct cal_format_info *find_format_by_pix(struct cal_ctx *ctx, 72 u32 pixelformat) 73 { 74 const struct cal_format_info *fmtinfo; 75 unsigned int k; 76 77 for (k = 0; k < ctx->num_active_fmt; k++) { 78 fmtinfo = ctx->active_fmt[k]; 79 if (fmtinfo->fourcc == pixelformat) 80 return fmtinfo; 81 } 82 83 return NULL; 84 } 85 86 static const struct cal_format_info *find_format_by_code(struct cal_ctx *ctx, 87 u32 code) 88 { 89 const struct cal_format_info *fmtinfo; 90 unsigned int k; 91 92 for (k = 0; k < ctx->num_active_fmt; k++) { 93 fmtinfo = ctx->active_fmt[k]; 94 if (fmtinfo->code == code) 95 return fmtinfo; 96 } 97 98 return NULL; 99 } 100 101 static int cal_legacy_enum_fmt_vid_cap(struct file *file, void *priv, 102 struct v4l2_fmtdesc *f) 103 { 104 struct cal_ctx *ctx = video_drvdata(file); 105 const struct cal_format_info *fmtinfo; 106 107 if (f->index >= ctx->num_active_fmt) 108 return -EINVAL; 109 110 fmtinfo = ctx->active_fmt[f->index]; 111 112 f->pixelformat = fmtinfo->fourcc; 113 f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 114 return 0; 115 } 116 117 static int __subdev_get_format(struct cal_ctx *ctx, 118 struct v4l2_mbus_framefmt *fmt) 119 { 120 struct v4l2_subdev_format sd_fmt; 121 struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format; 122 int ret; 123 124 sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; 125 sd_fmt.pad = 0; 126 127 ret = v4l2_subdev_call(ctx->phy->source, pad, get_fmt, NULL, &sd_fmt); 128 if (ret) 129 return ret; 130 131 *fmt = *mbus_fmt; 132 133 ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__, 134 fmt->width, fmt->height, fmt->code); 135 136 return 0; 137 } 138 139 static int __subdev_set_format(struct cal_ctx *ctx, 140 struct v4l2_mbus_framefmt *fmt) 141 { 142 struct v4l2_subdev_format sd_fmt; 143 struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format; 144 int ret; 145 146 sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; 147 sd_fmt.pad = 0; 148 *mbus_fmt = *fmt; 149 150 ret = v4l2_subdev_call(ctx->phy->source, pad, set_fmt, NULL, &sd_fmt); 151 if (ret) 152 return ret; 153 154 ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__, 155 fmt->width, fmt->height, fmt->code); 156 157 return 0; 158 } 159 160 static void cal_calc_format_size(struct cal_ctx *ctx, 161 const struct cal_format_info *fmtinfo, 162 struct v4l2_format *f) 163 { 164 u32 bpl, max_width; 165 166 /* 167 * Maximum width is bound by the DMA max width in bytes. 168 * We need to recalculate the actual maxi width depending on the 169 * number of bytes per pixels required. 170 */ 171 max_width = CAL_MAX_WIDTH_BYTES / (ALIGN(fmtinfo->bpp, 8) >> 3); 172 v4l_bound_align_image(&f->fmt.pix.width, 48, max_width, 2, 173 &f->fmt.pix.height, 32, CAL_MAX_HEIGHT_LINES, 174 0, 0); 175 176 bpl = (f->fmt.pix.width * ALIGN(fmtinfo->bpp, 8)) >> 3; 177 f->fmt.pix.bytesperline = ALIGN(bpl, 16); 178 179 f->fmt.pix.sizeimage = f->fmt.pix.height * 180 f->fmt.pix.bytesperline; 181 182 ctx_dbg(3, ctx, "%s: fourcc: %s size: %dx%d bpl:%d img_size:%d\n", 183 __func__, fourcc_to_str(f->fmt.pix.pixelformat), 184 f->fmt.pix.width, f->fmt.pix.height, 185 f->fmt.pix.bytesperline, f->fmt.pix.sizeimage); 186 } 187 188 static int cal_legacy_try_fmt_vid_cap(struct file *file, void *priv, 189 struct v4l2_format *f) 190 { 191 struct cal_ctx *ctx = video_drvdata(file); 192 const struct cal_format_info *fmtinfo; 193 struct v4l2_subdev_frame_size_enum fse; 194 int ret, found; 195 196 fmtinfo = find_format_by_pix(ctx, f->fmt.pix.pixelformat); 197 if (!fmtinfo) { 198 ctx_dbg(3, ctx, "Fourcc format (0x%08x) not found.\n", 199 f->fmt.pix.pixelformat); 200 201 /* Just get the first one enumerated */ 202 fmtinfo = ctx->active_fmt[0]; 203 f->fmt.pix.pixelformat = fmtinfo->fourcc; 204 } 205 206 f->fmt.pix.field = ctx->v_fmt.fmt.pix.field; 207 208 /* check for/find a valid width/height */ 209 ret = 0; 210 found = false; 211 fse.pad = 0; 212 fse.code = fmtinfo->code; 213 fse.which = V4L2_SUBDEV_FORMAT_ACTIVE; 214 for (fse.index = 0; ; fse.index++) { 215 ret = v4l2_subdev_call(ctx->phy->source, pad, enum_frame_size, 216 NULL, &fse); 217 if (ret) 218 break; 219 220 if ((f->fmt.pix.width == fse.max_width) && 221 (f->fmt.pix.height == fse.max_height)) { 222 found = true; 223 break; 224 } else if ((f->fmt.pix.width >= fse.min_width) && 225 (f->fmt.pix.width <= fse.max_width) && 226 (f->fmt.pix.height >= fse.min_height) && 227 (f->fmt.pix.height <= fse.max_height)) { 228 found = true; 229 break; 230 } 231 } 232 233 if (!found) { 234 /* use existing values as default */ 235 f->fmt.pix.width = ctx->v_fmt.fmt.pix.width; 236 f->fmt.pix.height = ctx->v_fmt.fmt.pix.height; 237 } 238 239 /* 240 * Use current colorspace for now, it will get 241 * updated properly during s_fmt 242 */ 243 f->fmt.pix.colorspace = ctx->v_fmt.fmt.pix.colorspace; 244 cal_calc_format_size(ctx, fmtinfo, f); 245 return 0; 246 } 247 248 static int cal_legacy_s_fmt_vid_cap(struct file *file, void *priv, 249 struct v4l2_format *f) 250 { 251 struct cal_ctx *ctx = video_drvdata(file); 252 struct vb2_queue *q = &ctx->vb_vidq; 253 struct v4l2_subdev_format sd_fmt = { 254 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 255 .pad = CAL_CAMERARX_PAD_SINK, 256 }; 257 const struct cal_format_info *fmtinfo; 258 int ret; 259 260 if (vb2_is_busy(q)) { 261 ctx_dbg(3, ctx, "%s device busy\n", __func__); 262 return -EBUSY; 263 } 264 265 ret = cal_legacy_try_fmt_vid_cap(file, priv, f); 266 if (ret < 0) 267 return ret; 268 269 fmtinfo = find_format_by_pix(ctx, f->fmt.pix.pixelformat); 270 271 v4l2_fill_mbus_format(&sd_fmt.format, &f->fmt.pix, fmtinfo->code); 272 273 ret = __subdev_set_format(ctx, &sd_fmt.format); 274 if (ret) 275 return ret; 276 277 /* Just double check nothing has gone wrong */ 278 if (sd_fmt.format.code != fmtinfo->code) { 279 ctx_dbg(3, ctx, 280 "%s subdev changed format on us, this should not happen\n", 281 __func__); 282 return -EINVAL; 283 } 284 285 v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &sd_fmt.format); 286 ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 287 ctx->v_fmt.fmt.pix.pixelformat = fmtinfo->fourcc; 288 ctx->v_fmt.fmt.pix.field = sd_fmt.format.field; 289 cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt); 290 291 v4l2_subdev_call(&ctx->phy->subdev, pad, set_fmt, NULL, &sd_fmt); 292 293 ctx->fmtinfo = fmtinfo; 294 *f = ctx->v_fmt; 295 296 return 0; 297 } 298 299 static int cal_legacy_enum_framesizes(struct file *file, void *fh, 300 struct v4l2_frmsizeenum *fsize) 301 { 302 struct cal_ctx *ctx = video_drvdata(file); 303 const struct cal_format_info *fmtinfo; 304 struct v4l2_subdev_frame_size_enum fse; 305 int ret; 306 307 /* check for valid format */ 308 fmtinfo = find_format_by_pix(ctx, fsize->pixel_format); 309 if (!fmtinfo) { 310 ctx_dbg(3, ctx, "Invalid pixel code: %x\n", 311 fsize->pixel_format); 312 return -EINVAL; 313 } 314 315 fse.index = fsize->index; 316 fse.pad = 0; 317 fse.code = fmtinfo->code; 318 fse.which = V4L2_SUBDEV_FORMAT_ACTIVE; 319 320 ret = v4l2_subdev_call(ctx->phy->source, pad, enum_frame_size, NULL, 321 &fse); 322 if (ret) 323 return ret; 324 325 ctx_dbg(1, ctx, "%s: index: %d code: %x W:[%d,%d] H:[%d,%d]\n", 326 __func__, fse.index, fse.code, fse.min_width, fse.max_width, 327 fse.min_height, fse.max_height); 328 329 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE; 330 fsize->discrete.width = fse.max_width; 331 fsize->discrete.height = fse.max_height; 332 333 return 0; 334 } 335 336 static int cal_legacy_enum_input(struct file *file, void *priv, 337 struct v4l2_input *inp) 338 { 339 if (inp->index > 0) 340 return -EINVAL; 341 342 inp->type = V4L2_INPUT_TYPE_CAMERA; 343 sprintf(inp->name, "Camera %u", inp->index); 344 return 0; 345 } 346 347 static int cal_legacy_g_input(struct file *file, void *priv, unsigned int *i) 348 { 349 *i = 0; 350 return 0; 351 } 352 353 static int cal_legacy_s_input(struct file *file, void *priv, unsigned int i) 354 { 355 return i > 0 ? -EINVAL : 0; 356 } 357 358 /* timeperframe is arbitrary and continuous */ 359 static int cal_legacy_enum_frameintervals(struct file *file, void *priv, 360 struct v4l2_frmivalenum *fival) 361 { 362 struct cal_ctx *ctx = video_drvdata(file); 363 const struct cal_format_info *fmtinfo; 364 struct v4l2_subdev_frame_interval_enum fie = { 365 .index = fival->index, 366 .width = fival->width, 367 .height = fival->height, 368 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 369 }; 370 int ret; 371 372 fmtinfo = find_format_by_pix(ctx, fival->pixel_format); 373 if (!fmtinfo) 374 return -EINVAL; 375 376 fie.code = fmtinfo->code; 377 ret = v4l2_subdev_call(ctx->phy->source, pad, enum_frame_interval, 378 NULL, &fie); 379 if (ret) 380 return ret; 381 fival->type = V4L2_FRMIVAL_TYPE_DISCRETE; 382 fival->discrete = fie.interval; 383 384 return 0; 385 } 386 387 static int cal_legacy_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a) 388 { 389 struct cal_ctx *ctx = video_drvdata(file); 390 391 return v4l2_g_parm_cap(video_devdata(file), ctx->phy->source, a); 392 } 393 394 static int cal_legacy_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a) 395 { 396 struct cal_ctx *ctx = video_drvdata(file); 397 398 return v4l2_s_parm_cap(video_devdata(file), ctx->phy->source, a); 399 } 400 401 static const struct v4l2_ioctl_ops cal_ioctl_legacy_ops = { 402 .vidioc_querycap = cal_querycap, 403 .vidioc_enum_fmt_vid_cap = cal_legacy_enum_fmt_vid_cap, 404 .vidioc_g_fmt_vid_cap = cal_g_fmt_vid_cap, 405 .vidioc_try_fmt_vid_cap = cal_legacy_try_fmt_vid_cap, 406 .vidioc_s_fmt_vid_cap = cal_legacy_s_fmt_vid_cap, 407 .vidioc_enum_framesizes = cal_legacy_enum_framesizes, 408 .vidioc_reqbufs = vb2_ioctl_reqbufs, 409 .vidioc_create_bufs = vb2_ioctl_create_bufs, 410 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 411 .vidioc_querybuf = vb2_ioctl_querybuf, 412 .vidioc_qbuf = vb2_ioctl_qbuf, 413 .vidioc_dqbuf = vb2_ioctl_dqbuf, 414 .vidioc_expbuf = vb2_ioctl_expbuf, 415 .vidioc_enum_input = cal_legacy_enum_input, 416 .vidioc_g_input = cal_legacy_g_input, 417 .vidioc_s_input = cal_legacy_s_input, 418 .vidioc_enum_frameintervals = cal_legacy_enum_frameintervals, 419 .vidioc_streamon = vb2_ioctl_streamon, 420 .vidioc_streamoff = vb2_ioctl_streamoff, 421 .vidioc_log_status = v4l2_ctrl_log_status, 422 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, 423 .vidioc_unsubscribe_event = v4l2_event_unsubscribe, 424 .vidioc_g_parm = cal_legacy_g_parm, 425 .vidioc_s_parm = cal_legacy_s_parm, 426 }; 427 428 /* ------------------------------------------------------------------ 429 * V4L2 Media Controller Centric IOCTLs 430 * ------------------------------------------------------------------ 431 */ 432 433 static int cal_mc_enum_fmt_vid_cap(struct file *file, void *priv, 434 struct v4l2_fmtdesc *f) 435 { 436 unsigned int i; 437 unsigned int idx; 438 439 if (f->index >= cal_num_formats) 440 return -EINVAL; 441 442 idx = 0; 443 444 for (i = 0; i < cal_num_formats; ++i) { 445 if (f->mbus_code && cal_formats[i].code != f->mbus_code) 446 continue; 447 448 if (idx == f->index) { 449 f->pixelformat = cal_formats[i].fourcc; 450 f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 451 return 0; 452 } 453 454 idx++; 455 } 456 457 return -EINVAL; 458 } 459 460 static void cal_mc_try_fmt(struct cal_ctx *ctx, struct v4l2_format *f, 461 const struct cal_format_info **info) 462 { 463 struct v4l2_pix_format *format = &f->fmt.pix; 464 const struct cal_format_info *fmtinfo; 465 unsigned int bpp; 466 467 /* 468 * Default to the first format if the requested pixel format code isn't 469 * supported. 470 */ 471 fmtinfo = cal_format_by_fourcc(f->fmt.pix.pixelformat); 472 if (!fmtinfo) 473 fmtinfo = &cal_formats[0]; 474 475 /* 476 * Clamp the size, update the pixel format. The field and colorspace are 477 * accepted as-is, except for V4L2_FIELD_ANY that is turned into 478 * V4L2_FIELD_NONE. 479 */ 480 bpp = ALIGN(fmtinfo->bpp, 8); 481 482 format->width = clamp_t(unsigned int, format->width, 483 CAL_MIN_WIDTH_BYTES * 8 / bpp, 484 CAL_MAX_WIDTH_BYTES * 8 / bpp); 485 format->height = clamp_t(unsigned int, format->height, 486 CAL_MIN_HEIGHT_LINES, CAL_MAX_HEIGHT_LINES); 487 format->pixelformat = fmtinfo->fourcc; 488 489 if (format->field == V4L2_FIELD_ANY) 490 format->field = V4L2_FIELD_NONE; 491 492 /* 493 * Calculate the number of bytes per line and the image size. The 494 * hardware stores the stride as a number of 16 bytes words, in a 495 * signed 15-bit value. Only 14 bits are thus usable. 496 */ 497 format->bytesperline = ALIGN(clamp(format->bytesperline, 498 format->width * bpp / 8, 499 ((1U << 14) - 1) * 16), 16); 500 501 format->sizeimage = format->height * format->bytesperline; 502 503 format->colorspace = ctx->v_fmt.fmt.pix.colorspace; 504 505 if (info) 506 *info = fmtinfo; 507 508 ctx_dbg(3, ctx, "%s: %s %ux%u (bytesperline %u sizeimage %u)\n", 509 __func__, fourcc_to_str(format->pixelformat), 510 format->width, format->height, 511 format->bytesperline, format->sizeimage); 512 } 513 514 static int cal_mc_try_fmt_vid_cap(struct file *file, void *priv, 515 struct v4l2_format *f) 516 { 517 struct cal_ctx *ctx = video_drvdata(file); 518 519 cal_mc_try_fmt(ctx, f, NULL); 520 return 0; 521 } 522 523 static int cal_mc_s_fmt_vid_cap(struct file *file, void *priv, 524 struct v4l2_format *f) 525 { 526 struct cal_ctx *ctx = video_drvdata(file); 527 const struct cal_format_info *fmtinfo; 528 529 if (vb2_is_busy(&ctx->vb_vidq)) { 530 ctx_dbg(3, ctx, "%s device busy\n", __func__); 531 return -EBUSY; 532 } 533 534 cal_mc_try_fmt(ctx, f, &fmtinfo); 535 536 ctx->v_fmt = *f; 537 ctx->fmtinfo = fmtinfo; 538 539 return 0; 540 } 541 542 static int cal_mc_enum_framesizes(struct file *file, void *fh, 543 struct v4l2_frmsizeenum *fsize) 544 { 545 struct cal_ctx *ctx = video_drvdata(file); 546 const struct cal_format_info *fmtinfo; 547 unsigned int bpp; 548 549 if (fsize->index > 0) 550 return -EINVAL; 551 552 fmtinfo = cal_format_by_fourcc(fsize->pixel_format); 553 if (!fmtinfo) { 554 ctx_dbg(3, ctx, "Invalid pixel format 0x%08x\n", 555 fsize->pixel_format); 556 return -EINVAL; 557 } 558 559 bpp = ALIGN(fmtinfo->bpp, 8); 560 561 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; 562 fsize->stepwise.min_width = CAL_MIN_WIDTH_BYTES * 8 / bpp; 563 fsize->stepwise.max_width = CAL_MAX_WIDTH_BYTES * 8 / bpp; 564 fsize->stepwise.step_width = 64 / bpp; 565 fsize->stepwise.min_height = CAL_MIN_HEIGHT_LINES; 566 fsize->stepwise.max_height = CAL_MAX_HEIGHT_LINES; 567 fsize->stepwise.step_height = 1; 568 569 return 0; 570 } 571 572 static const struct v4l2_ioctl_ops cal_ioctl_mc_ops = { 573 .vidioc_querycap = cal_querycap, 574 .vidioc_enum_fmt_vid_cap = cal_mc_enum_fmt_vid_cap, 575 .vidioc_g_fmt_vid_cap = cal_g_fmt_vid_cap, 576 .vidioc_try_fmt_vid_cap = cal_mc_try_fmt_vid_cap, 577 .vidioc_s_fmt_vid_cap = cal_mc_s_fmt_vid_cap, 578 .vidioc_enum_framesizes = cal_mc_enum_framesizes, 579 .vidioc_reqbufs = vb2_ioctl_reqbufs, 580 .vidioc_create_bufs = vb2_ioctl_create_bufs, 581 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 582 .vidioc_querybuf = vb2_ioctl_querybuf, 583 .vidioc_qbuf = vb2_ioctl_qbuf, 584 .vidioc_dqbuf = vb2_ioctl_dqbuf, 585 .vidioc_expbuf = vb2_ioctl_expbuf, 586 .vidioc_streamon = vb2_ioctl_streamon, 587 .vidioc_streamoff = vb2_ioctl_streamoff, 588 .vidioc_log_status = v4l2_ctrl_log_status, 589 }; 590 591 /* ------------------------------------------------------------------ 592 * videobuf2 Common Operations 593 * ------------------------------------------------------------------ 594 */ 595 596 static int cal_queue_setup(struct vb2_queue *vq, 597 unsigned int *nbuffers, unsigned int *nplanes, 598 unsigned int sizes[], struct device *alloc_devs[]) 599 { 600 struct cal_ctx *ctx = vb2_get_drv_priv(vq); 601 unsigned int size = ctx->v_fmt.fmt.pix.sizeimage; 602 603 if (vq->num_buffers + *nbuffers < 3) 604 *nbuffers = 3 - vq->num_buffers; 605 606 if (*nplanes) { 607 if (sizes[0] < size) 608 return -EINVAL; 609 size = sizes[0]; 610 } 611 612 *nplanes = 1; 613 sizes[0] = size; 614 615 ctx_dbg(3, ctx, "nbuffers=%d, size=%d\n", *nbuffers, sizes[0]); 616 617 return 0; 618 } 619 620 static int cal_buffer_prepare(struct vb2_buffer *vb) 621 { 622 struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 623 struct cal_buffer *buf = container_of(vb, struct cal_buffer, 624 vb.vb2_buf); 625 unsigned long size; 626 627 size = ctx->v_fmt.fmt.pix.sizeimage; 628 if (vb2_plane_size(vb, 0) < size) { 629 ctx_err(ctx, 630 "data will not fit into plane (%lu < %lu)\n", 631 vb2_plane_size(vb, 0), size); 632 return -EINVAL; 633 } 634 635 vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size); 636 return 0; 637 } 638 639 static void cal_buffer_queue(struct vb2_buffer *vb) 640 { 641 struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 642 struct cal_buffer *buf = container_of(vb, struct cal_buffer, 643 vb.vb2_buf); 644 unsigned long flags; 645 646 /* recheck locking */ 647 spin_lock_irqsave(&ctx->dma.lock, flags); 648 list_add_tail(&buf->list, &ctx->dma.queue); 649 spin_unlock_irqrestore(&ctx->dma.lock, flags); 650 } 651 652 static void cal_release_buffers(struct cal_ctx *ctx, 653 enum vb2_buffer_state state) 654 { 655 struct cal_buffer *buf, *tmp; 656 657 /* Release all queued buffers. */ 658 spin_lock_irq(&ctx->dma.lock); 659 660 list_for_each_entry_safe(buf, tmp, &ctx->dma.queue, list) { 661 list_del(&buf->list); 662 vb2_buffer_done(&buf->vb.vb2_buf, state); 663 } 664 665 if (ctx->dma.pending) { 666 vb2_buffer_done(&ctx->dma.pending->vb.vb2_buf, state); 667 ctx->dma.pending = NULL; 668 } 669 670 if (ctx->dma.active) { 671 vb2_buffer_done(&ctx->dma.active->vb.vb2_buf, state); 672 ctx->dma.active = NULL; 673 } 674 675 spin_unlock_irq(&ctx->dma.lock); 676 } 677 678 /* ------------------------------------------------------------------ 679 * videobuf2 Operations 680 * ------------------------------------------------------------------ 681 */ 682 683 static int cal_video_check_format(struct cal_ctx *ctx) 684 { 685 const struct v4l2_mbus_framefmt *format; 686 struct media_pad *remote_pad; 687 688 remote_pad = media_pad_remote_pad_first(&ctx->pad); 689 if (!remote_pad) 690 return -ENODEV; 691 692 format = &ctx->phy->formats[remote_pad->index]; 693 694 if (ctx->fmtinfo->code != format->code || 695 ctx->v_fmt.fmt.pix.height != format->height || 696 ctx->v_fmt.fmt.pix.width != format->width || 697 ctx->v_fmt.fmt.pix.field != format->field) 698 return -EPIPE; 699 700 return 0; 701 } 702 703 static int cal_start_streaming(struct vb2_queue *vq, unsigned int count) 704 { 705 struct cal_ctx *ctx = vb2_get_drv_priv(vq); 706 struct cal_buffer *buf; 707 dma_addr_t addr; 708 int ret; 709 710 ret = media_pipeline_start(&ctx->vdev.entity, &ctx->phy->pipe); 711 if (ret < 0) { 712 ctx_err(ctx, "Failed to start media pipeline: %d\n", ret); 713 goto error_release_buffers; 714 } 715 716 /* 717 * Verify that the currently configured format matches the output of 718 * the connected CAMERARX. 719 */ 720 ret = cal_video_check_format(ctx); 721 if (ret < 0) { 722 ctx_dbg(3, ctx, 723 "Format mismatch between CAMERARX and video node\n"); 724 goto error_pipeline; 725 } 726 727 ret = cal_ctx_prepare(ctx); 728 if (ret) { 729 ctx_err(ctx, "Failed to prepare context: %d\n", ret); 730 goto error_pipeline; 731 } 732 733 spin_lock_irq(&ctx->dma.lock); 734 buf = list_first_entry(&ctx->dma.queue, struct cal_buffer, list); 735 ctx->dma.active = buf; 736 list_del(&buf->list); 737 spin_unlock_irq(&ctx->dma.lock); 738 739 addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0); 740 741 ret = pm_runtime_resume_and_get(ctx->cal->dev); 742 if (ret < 0) 743 goto error_pipeline; 744 745 cal_ctx_set_dma_addr(ctx, addr); 746 cal_ctx_start(ctx); 747 748 ret = v4l2_subdev_call(&ctx->phy->subdev, video, s_stream, 1); 749 if (ret) 750 goto error_stop; 751 752 if (cal_debug >= 4) 753 cal_quickdump_regs(ctx->cal); 754 755 return 0; 756 757 error_stop: 758 cal_ctx_stop(ctx); 759 pm_runtime_put_sync(ctx->cal->dev); 760 cal_ctx_unprepare(ctx); 761 762 error_pipeline: 763 media_pipeline_stop(&ctx->vdev.entity); 764 error_release_buffers: 765 cal_release_buffers(ctx, VB2_BUF_STATE_QUEUED); 766 767 return ret; 768 } 769 770 static void cal_stop_streaming(struct vb2_queue *vq) 771 { 772 struct cal_ctx *ctx = vb2_get_drv_priv(vq); 773 774 cal_ctx_stop(ctx); 775 776 v4l2_subdev_call(&ctx->phy->subdev, video, s_stream, 0); 777 778 pm_runtime_put_sync(ctx->cal->dev); 779 780 cal_ctx_unprepare(ctx); 781 782 cal_release_buffers(ctx, VB2_BUF_STATE_ERROR); 783 784 media_pipeline_stop(&ctx->vdev.entity); 785 } 786 787 static const struct vb2_ops cal_video_qops = { 788 .queue_setup = cal_queue_setup, 789 .buf_prepare = cal_buffer_prepare, 790 .buf_queue = cal_buffer_queue, 791 .start_streaming = cal_start_streaming, 792 .stop_streaming = cal_stop_streaming, 793 .wait_prepare = vb2_ops_wait_prepare, 794 .wait_finish = vb2_ops_wait_finish, 795 }; 796 797 /* ------------------------------------------------------------------ 798 * V4L2 Initialization and Registration 799 * ------------------------------------------------------------------ 800 */ 801 802 static const struct v4l2_file_operations cal_fops = { 803 .owner = THIS_MODULE, 804 .open = v4l2_fh_open, 805 .release = vb2_fop_release, 806 .poll = vb2_fop_poll, 807 .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */ 808 .mmap = vb2_fop_mmap, 809 }; 810 811 static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx) 812 { 813 struct v4l2_subdev_mbus_code_enum mbus_code; 814 struct v4l2_mbus_framefmt mbus_fmt; 815 const struct cal_format_info *fmtinfo; 816 unsigned int i, j, k; 817 int ret = 0; 818 819 /* Enumerate sub device formats and enable all matching local formats */ 820 ctx->active_fmt = devm_kcalloc(ctx->cal->dev, cal_num_formats, 821 sizeof(*ctx->active_fmt), GFP_KERNEL); 822 if (!ctx->active_fmt) 823 return -ENOMEM; 824 825 ctx->num_active_fmt = 0; 826 827 for (j = 0, i = 0; ; ++j) { 828 829 memset(&mbus_code, 0, sizeof(mbus_code)); 830 mbus_code.index = j; 831 mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE; 832 ret = v4l2_subdev_call(ctx->phy->source, pad, enum_mbus_code, 833 NULL, &mbus_code); 834 if (ret == -EINVAL) 835 break; 836 837 if (ret) { 838 ctx_err(ctx, "Error enumerating mbus codes in subdev %s: %d\n", 839 ctx->phy->source->name, ret); 840 return ret; 841 } 842 843 ctx_dbg(2, ctx, 844 "subdev %s: code: %04x idx: %u\n", 845 ctx->phy->source->name, mbus_code.code, j); 846 847 for (k = 0; k < cal_num_formats; k++) { 848 fmtinfo = &cal_formats[k]; 849 850 if (mbus_code.code == fmtinfo->code) { 851 ctx->active_fmt[i] = fmtinfo; 852 ctx_dbg(2, ctx, 853 "matched fourcc: %s: code: %04x idx: %u\n", 854 fourcc_to_str(fmtinfo->fourcc), 855 fmtinfo->code, i); 856 ctx->num_active_fmt = ++i; 857 } 858 } 859 } 860 861 if (i == 0) { 862 ctx_err(ctx, "No suitable format reported by subdev %s\n", 863 ctx->phy->source->name); 864 return -EINVAL; 865 } 866 867 ret = __subdev_get_format(ctx, &mbus_fmt); 868 if (ret) 869 return ret; 870 871 fmtinfo = find_format_by_code(ctx, mbus_fmt.code); 872 if (!fmtinfo) { 873 ctx_dbg(3, ctx, "mbus code format (0x%08x) not found.\n", 874 mbus_fmt.code); 875 return -EINVAL; 876 } 877 878 /* Save current format */ 879 v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt); 880 ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 881 ctx->v_fmt.fmt.pix.pixelformat = fmtinfo->fourcc; 882 cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt); 883 ctx->fmtinfo = fmtinfo; 884 885 return 0; 886 } 887 888 static int cal_ctx_v4l2_init_mc_format(struct cal_ctx *ctx) 889 { 890 const struct cal_format_info *fmtinfo; 891 struct v4l2_pix_format *pix_fmt = &ctx->v_fmt.fmt.pix; 892 893 fmtinfo = cal_format_by_code(MEDIA_BUS_FMT_UYVY8_2X8); 894 if (!fmtinfo) 895 return -EINVAL; 896 897 pix_fmt->width = 640; 898 pix_fmt->height = 480; 899 pix_fmt->field = V4L2_FIELD_NONE; 900 pix_fmt->colorspace = V4L2_COLORSPACE_SRGB; 901 pix_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601; 902 pix_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE; 903 pix_fmt->xfer_func = V4L2_XFER_FUNC_SRGB; 904 pix_fmt->pixelformat = fmtinfo->fourcc; 905 906 ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 907 908 /* Save current format */ 909 cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt); 910 ctx->fmtinfo = fmtinfo; 911 912 return 0; 913 } 914 915 int cal_ctx_v4l2_register(struct cal_ctx *ctx) 916 { 917 struct video_device *vfd = &ctx->vdev; 918 int ret; 919 920 if (!cal_mc_api) { 921 struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler; 922 923 ret = cal_ctx_v4l2_init_formats(ctx); 924 if (ret) { 925 ctx_err(ctx, "Failed to init formats: %d\n", ret); 926 return ret; 927 } 928 929 ret = v4l2_ctrl_add_handler(hdl, ctx->phy->source->ctrl_handler, 930 NULL, true); 931 if (ret < 0) { 932 ctx_err(ctx, "Failed to add source ctrl handler\n"); 933 return ret; 934 } 935 } else { 936 ret = cal_ctx_v4l2_init_mc_format(ctx); 937 if (ret) { 938 ctx_err(ctx, "Failed to init format: %d\n", ret); 939 return ret; 940 } 941 } 942 943 ret = video_register_device(vfd, VFL_TYPE_VIDEO, cal_video_nr); 944 if (ret < 0) { 945 ctx_err(ctx, "Failed to register video device\n"); 946 return ret; 947 } 948 949 ret = media_create_pad_link(&ctx->phy->subdev.entity, 950 CAL_CAMERARX_PAD_FIRST_SOURCE, 951 &vfd->entity, 0, 952 MEDIA_LNK_FL_IMMUTABLE | 953 MEDIA_LNK_FL_ENABLED); 954 if (ret) { 955 ctx_err(ctx, "Failed to create media link for context %u\n", 956 ctx->dma_ctx); 957 video_unregister_device(vfd); 958 return ret; 959 } 960 961 ctx_info(ctx, "V4L2 device registered as %s\n", 962 video_device_node_name(vfd)); 963 964 return 0; 965 } 966 967 void cal_ctx_v4l2_unregister(struct cal_ctx *ctx) 968 { 969 ctx_dbg(1, ctx, "unregistering %s\n", 970 video_device_node_name(&ctx->vdev)); 971 972 video_unregister_device(&ctx->vdev); 973 } 974 975 int cal_ctx_v4l2_init(struct cal_ctx *ctx) 976 { 977 struct video_device *vfd = &ctx->vdev; 978 struct vb2_queue *q = &ctx->vb_vidq; 979 int ret; 980 981 INIT_LIST_HEAD(&ctx->dma.queue); 982 spin_lock_init(&ctx->dma.lock); 983 mutex_init(&ctx->mutex); 984 init_waitqueue_head(&ctx->dma.wait); 985 986 /* Initialize the vb2 queue. */ 987 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 988 q->io_modes = VB2_MMAP | VB2_DMABUF; 989 q->drv_priv = ctx; 990 q->buf_struct_size = sizeof(struct cal_buffer); 991 q->ops = &cal_video_qops; 992 q->mem_ops = &vb2_dma_contig_memops; 993 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 994 q->lock = &ctx->mutex; 995 q->min_buffers_needed = 3; 996 q->dev = ctx->cal->dev; 997 998 ret = vb2_queue_init(q); 999 if (ret) 1000 return ret; 1001 1002 /* Initialize the video device and media entity. */ 1003 vfd->fops = &cal_fops; 1004 vfd->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING 1005 | (cal_mc_api ? V4L2_CAP_IO_MC : 0); 1006 vfd->v4l2_dev = &ctx->cal->v4l2_dev; 1007 vfd->queue = q; 1008 snprintf(vfd->name, sizeof(vfd->name), "CAL output %u", ctx->dma_ctx); 1009 vfd->release = video_device_release_empty; 1010 vfd->ioctl_ops = cal_mc_api ? &cal_ioctl_mc_ops : &cal_ioctl_legacy_ops; 1011 vfd->lock = &ctx->mutex; 1012 video_set_drvdata(vfd, ctx); 1013 1014 ctx->pad.flags = MEDIA_PAD_FL_SINK; 1015 ret = media_entity_pads_init(&vfd->entity, 1, &ctx->pad); 1016 if (ret < 0) 1017 return ret; 1018 1019 if (!cal_mc_api) { 1020 /* Initialize the control handler. */ 1021 struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler; 1022 1023 ret = v4l2_ctrl_handler_init(hdl, 11); 1024 if (ret < 0) { 1025 ctx_err(ctx, "Failed to init ctrl handler\n"); 1026 goto error; 1027 } 1028 1029 vfd->ctrl_handler = hdl; 1030 } 1031 1032 return 0; 1033 1034 error: 1035 media_entity_cleanup(&vfd->entity); 1036 return ret; 1037 } 1038 1039 void cal_ctx_v4l2_cleanup(struct cal_ctx *ctx) 1040 { 1041 if (!cal_mc_api) 1042 v4l2_ctrl_handler_free(&ctx->ctrl_handler); 1043 1044 media_entity_cleanup(&ctx->vdev.entity); 1045 } 1046