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