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/delay.h> 13#include <linux/ioctl.h> 14#include <linux/pm_runtime.h> 15#include <linux/videodev2.h> 16 17#include <media/media-device.h> 18#include <media/v4l2-common.h> 19#include <media/v4l2-ctrls.h> 20#include <media/v4l2-device.h> 21#include <media/v4l2-event.h> 22#include <media/v4l2-fh.h> 23#include <media/v4l2-ioctl.h> 24#include <media/videobuf2-core.h> 25#include <media/videobuf2-dma-contig.h> 26 27#include "cal.h" 28 29/* ------------------------------------------------------------------ 30 * Format Handling 31 * ------------------------------------------------------------------ 32 */ 33 34static const struct cal_fmt cal_formats[] = { 35 { 36 .fourcc = V4L2_PIX_FMT_YUYV, 37 .code = MEDIA_BUS_FMT_YUYV8_2X8, 38 .bpp = 16, 39 }, { 40 .fourcc = V4L2_PIX_FMT_UYVY, 41 .code = MEDIA_BUS_FMT_UYVY8_2X8, 42 .bpp = 16, 43 }, { 44 .fourcc = V4L2_PIX_FMT_YVYU, 45 .code = MEDIA_BUS_FMT_YVYU8_2X8, 46 .bpp = 16, 47 }, { 48 .fourcc = V4L2_PIX_FMT_VYUY, 49 .code = MEDIA_BUS_FMT_VYUY8_2X8, 50 .bpp = 16, 51 }, { 52 .fourcc = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */ 53 .code = MEDIA_BUS_FMT_RGB565_2X8_LE, 54 .bpp = 16, 55 }, { 56 .fourcc = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */ 57 .code = MEDIA_BUS_FMT_RGB565_2X8_BE, 58 .bpp = 16, 59 }, { 60 .fourcc = V4L2_PIX_FMT_RGB555, /* gggbbbbb arrrrrgg */ 61 .code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, 62 .bpp = 16, 63 }, { 64 .fourcc = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */ 65 .code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, 66 .bpp = 16, 67 }, { 68 .fourcc = V4L2_PIX_FMT_RGB24, /* rgb */ 69 .code = MEDIA_BUS_FMT_RGB888_2X12_LE, 70 .bpp = 24, 71 }, { 72 .fourcc = V4L2_PIX_FMT_BGR24, /* bgr */ 73 .code = MEDIA_BUS_FMT_RGB888_2X12_BE, 74 .bpp = 24, 75 }, { 76 .fourcc = V4L2_PIX_FMT_RGB32, /* argb */ 77 .code = MEDIA_BUS_FMT_ARGB8888_1X32, 78 .bpp = 32, 79 }, { 80 .fourcc = V4L2_PIX_FMT_SBGGR8, 81 .code = MEDIA_BUS_FMT_SBGGR8_1X8, 82 .bpp = 8, 83 }, { 84 .fourcc = V4L2_PIX_FMT_SGBRG8, 85 .code = MEDIA_BUS_FMT_SGBRG8_1X8, 86 .bpp = 8, 87 }, { 88 .fourcc = V4L2_PIX_FMT_SGRBG8, 89 .code = MEDIA_BUS_FMT_SGRBG8_1X8, 90 .bpp = 8, 91 }, { 92 .fourcc = V4L2_PIX_FMT_SRGGB8, 93 .code = MEDIA_BUS_FMT_SRGGB8_1X8, 94 .bpp = 8, 95 }, { 96 .fourcc = V4L2_PIX_FMT_SBGGR10, 97 .code = MEDIA_BUS_FMT_SBGGR10_1X10, 98 .bpp = 10, 99 }, { 100 .fourcc = V4L2_PIX_FMT_SGBRG10, 101 .code = MEDIA_BUS_FMT_SGBRG10_1X10, 102 .bpp = 10, 103 }, { 104 .fourcc = V4L2_PIX_FMT_SGRBG10, 105 .code = MEDIA_BUS_FMT_SGRBG10_1X10, 106 .bpp = 10, 107 }, { 108 .fourcc = V4L2_PIX_FMT_SRGGB10, 109 .code = MEDIA_BUS_FMT_SRGGB10_1X10, 110 .bpp = 10, 111 }, { 112 .fourcc = V4L2_PIX_FMT_SBGGR12, 113 .code = MEDIA_BUS_FMT_SBGGR12_1X12, 114 .bpp = 12, 115 }, { 116 .fourcc = V4L2_PIX_FMT_SGBRG12, 117 .code = MEDIA_BUS_FMT_SGBRG12_1X12, 118 .bpp = 12, 119 }, { 120 .fourcc = V4L2_PIX_FMT_SGRBG12, 121 .code = MEDIA_BUS_FMT_SGRBG12_1X12, 122 .bpp = 12, 123 }, { 124 .fourcc = V4L2_PIX_FMT_SRGGB12, 125 .code = MEDIA_BUS_FMT_SRGGB12_1X12, 126 .bpp = 12, 127 }, 128}; 129 130/* Print Four-character-code (FOURCC) */ 131static char *fourcc_to_str(u32 fmt) 132{ 133 static char code[5]; 134 135 code[0] = (unsigned char)(fmt & 0xff); 136 code[1] = (unsigned char)((fmt >> 8) & 0xff); 137 code[2] = (unsigned char)((fmt >> 16) & 0xff); 138 code[3] = (unsigned char)((fmt >> 24) & 0xff); 139 code[4] = '\0'; 140 141 return code; 142} 143 144/* ------------------------------------------------------------------ 145 * V4L2 Video IOCTLs 146 * ------------------------------------------------------------------ 147 */ 148 149static const struct cal_fmt *find_format_by_pix(struct cal_ctx *ctx, 150 u32 pixelformat) 151{ 152 const struct cal_fmt *fmt; 153 unsigned int k; 154 155 for (k = 0; k < ctx->num_active_fmt; k++) { 156 fmt = ctx->active_fmt[k]; 157 if (fmt->fourcc == pixelformat) 158 return fmt; 159 } 160 161 return NULL; 162} 163 164static const struct cal_fmt *find_format_by_code(struct cal_ctx *ctx, 165 u32 code) 166{ 167 const struct cal_fmt *fmt; 168 unsigned int k; 169 170 for (k = 0; k < ctx->num_active_fmt; k++) { 171 fmt = ctx->active_fmt[k]; 172 if (fmt->code == code) 173 return fmt; 174 } 175 176 return NULL; 177} 178 179static int cal_querycap(struct file *file, void *priv, 180 struct v4l2_capability *cap) 181{ 182 struct cal_ctx *ctx = video_drvdata(file); 183 184 strscpy(cap->driver, CAL_MODULE_NAME, sizeof(cap->driver)); 185 strscpy(cap->card, CAL_MODULE_NAME, sizeof(cap->card)); 186 187 snprintf(cap->bus_info, sizeof(cap->bus_info), 188 "platform:%s", dev_name(ctx->cal->dev)); 189 return 0; 190} 191 192static int cal_enum_fmt_vid_cap(struct file *file, void *priv, 193 struct v4l2_fmtdesc *f) 194{ 195 struct cal_ctx *ctx = video_drvdata(file); 196 const struct cal_fmt *fmt; 197 198 if (f->index >= ctx->num_active_fmt) 199 return -EINVAL; 200 201 fmt = ctx->active_fmt[f->index]; 202 203 f->pixelformat = fmt->fourcc; 204 f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 205 return 0; 206} 207 208static int __subdev_get_format(struct cal_ctx *ctx, 209 struct v4l2_mbus_framefmt *fmt) 210{ 211 struct v4l2_subdev_format sd_fmt; 212 struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format; 213 int ret; 214 215 sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; 216 sd_fmt.pad = 0; 217 218 ret = v4l2_subdev_call(ctx->phy->sensor, pad, get_fmt, NULL, &sd_fmt); 219 if (ret) 220 return ret; 221 222 *fmt = *mbus_fmt; 223 224 ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__, 225 fmt->width, fmt->height, fmt->code); 226 227 return 0; 228} 229 230static int __subdev_set_format(struct cal_ctx *ctx, 231 struct v4l2_mbus_framefmt *fmt) 232{ 233 struct v4l2_subdev_format sd_fmt; 234 struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format; 235 int ret; 236 237 sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; 238 sd_fmt.pad = 0; 239 *mbus_fmt = *fmt; 240 241 ret = v4l2_subdev_call(ctx->phy->sensor, pad, set_fmt, NULL, &sd_fmt); 242 if (ret) 243 return ret; 244 245 ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__, 246 fmt->width, fmt->height, fmt->code); 247 248 return 0; 249} 250 251static int cal_calc_format_size(struct cal_ctx *ctx, 252 const struct cal_fmt *fmt, 253 struct v4l2_format *f) 254{ 255 u32 bpl, max_width; 256 257 if (!fmt) { 258 ctx_dbg(3, ctx, "No cal_fmt provided!\n"); 259 return -EINVAL; 260 } 261 262 /* 263 * Maximum width is bound by the DMA max width in bytes. 264 * We need to recalculate the actual maxi width depending on the 265 * number of bytes per pixels required. 266 */ 267 max_width = MAX_WIDTH_BYTES / (ALIGN(fmt->bpp, 8) >> 3); 268 v4l_bound_align_image(&f->fmt.pix.width, 48, max_width, 2, 269 &f->fmt.pix.height, 32, MAX_HEIGHT_LINES, 0, 0); 270 271 bpl = (f->fmt.pix.width * ALIGN(fmt->bpp, 8)) >> 3; 272 f->fmt.pix.bytesperline = ALIGN(bpl, 16); 273 274 f->fmt.pix.sizeimage = f->fmt.pix.height * 275 f->fmt.pix.bytesperline; 276 277 ctx_dbg(3, ctx, "%s: fourcc: %s size: %dx%d bpl:%d img_size:%d\n", 278 __func__, fourcc_to_str(f->fmt.pix.pixelformat), 279 f->fmt.pix.width, f->fmt.pix.height, 280 f->fmt.pix.bytesperline, f->fmt.pix.sizeimage); 281 282 return 0; 283} 284 285static int cal_g_fmt_vid_cap(struct file *file, void *priv, 286 struct v4l2_format *f) 287{ 288 struct cal_ctx *ctx = video_drvdata(file); 289 290 *f = ctx->v_fmt; 291 292 return 0; 293} 294 295static int cal_try_fmt_vid_cap(struct file *file, void *priv, 296 struct v4l2_format *f) 297{ 298 struct cal_ctx *ctx = video_drvdata(file); 299 const struct cal_fmt *fmt; 300 struct v4l2_subdev_frame_size_enum fse; 301 int ret, found; 302 303 fmt = find_format_by_pix(ctx, f->fmt.pix.pixelformat); 304 if (!fmt) { 305 ctx_dbg(3, ctx, "Fourcc format (0x%08x) not found.\n", 306 f->fmt.pix.pixelformat); 307 308 /* Just get the first one enumerated */ 309 fmt = ctx->active_fmt[0]; 310 f->fmt.pix.pixelformat = fmt->fourcc; 311 } 312 313 f->fmt.pix.field = ctx->v_fmt.fmt.pix.field; 314 315 /* check for/find a valid width/height */ 316 ret = 0; 317 found = false; 318 fse.pad = 0; 319 fse.code = fmt->code; 320 fse.which = V4L2_SUBDEV_FORMAT_ACTIVE; 321 for (fse.index = 0; ; fse.index++) { 322 ret = v4l2_subdev_call(ctx->phy->sensor, pad, enum_frame_size, 323 NULL, &fse); 324 if (ret) 325 break; 326 327 if ((f->fmt.pix.width == fse.max_width) && 328 (f->fmt.pix.height == fse.max_height)) { 329 found = true; 330 break; 331 } else if ((f->fmt.pix.width >= fse.min_width) && 332 (f->fmt.pix.width <= fse.max_width) && 333 (f->fmt.pix.height >= fse.min_height) && 334 (f->fmt.pix.height <= fse.max_height)) { 335 found = true; 336 break; 337 } 338 } 339 340 if (!found) { 341 /* use existing values as default */ 342 f->fmt.pix.width = ctx->v_fmt.fmt.pix.width; 343 f->fmt.pix.height = ctx->v_fmt.fmt.pix.height; 344 } 345 346 /* 347 * Use current colorspace for now, it will get 348 * updated properly during s_fmt 349 */ 350 f->fmt.pix.colorspace = ctx->v_fmt.fmt.pix.colorspace; 351 return cal_calc_format_size(ctx, fmt, f); 352} 353 354static int cal_s_fmt_vid_cap(struct file *file, void *priv, 355 struct v4l2_format *f) 356{ 357 struct cal_ctx *ctx = video_drvdata(file); 358 struct vb2_queue *q = &ctx->vb_vidq; 359 const struct cal_fmt *fmt; 360 struct v4l2_mbus_framefmt mbus_fmt; 361 int ret; 362 363 if (vb2_is_busy(q)) { 364 ctx_dbg(3, ctx, "%s device busy\n", __func__); 365 return -EBUSY; 366 } 367 368 ret = cal_try_fmt_vid_cap(file, priv, f); 369 if (ret < 0) 370 return ret; 371 372 fmt = find_format_by_pix(ctx, f->fmt.pix.pixelformat); 373 374 v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, fmt->code); 375 376 ret = __subdev_set_format(ctx, &mbus_fmt); 377 if (ret) 378 return ret; 379 380 /* Just double check nothing has gone wrong */ 381 if (mbus_fmt.code != fmt->code) { 382 ctx_dbg(3, ctx, 383 "%s subdev changed format on us, this should not happen\n", 384 __func__); 385 return -EINVAL; 386 } 387 388 v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt); 389 ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 390 ctx->v_fmt.fmt.pix.pixelformat = fmt->fourcc; 391 cal_calc_format_size(ctx, fmt, &ctx->v_fmt); 392 ctx->fmt = fmt; 393 ctx->m_fmt = mbus_fmt; 394 *f = ctx->v_fmt; 395 396 return 0; 397} 398 399static int cal_enum_framesizes(struct file *file, void *fh, 400 struct v4l2_frmsizeenum *fsize) 401{ 402 struct cal_ctx *ctx = video_drvdata(file); 403 const struct cal_fmt *fmt; 404 struct v4l2_subdev_frame_size_enum fse; 405 int ret; 406 407 /* check for valid format */ 408 fmt = find_format_by_pix(ctx, fsize->pixel_format); 409 if (!fmt) { 410 ctx_dbg(3, ctx, "Invalid pixel code: %x\n", 411 fsize->pixel_format); 412 return -EINVAL; 413 } 414 415 fse.index = fsize->index; 416 fse.pad = 0; 417 fse.code = fmt->code; 418 fse.which = V4L2_SUBDEV_FORMAT_ACTIVE; 419 420 ret = v4l2_subdev_call(ctx->phy->sensor, pad, enum_frame_size, NULL, 421 &fse); 422 if (ret) 423 return ret; 424 425 ctx_dbg(1, ctx, "%s: index: %d code: %x W:[%d,%d] H:[%d,%d]\n", 426 __func__, fse.index, fse.code, fse.min_width, fse.max_width, 427 fse.min_height, fse.max_height); 428 429 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE; 430 fsize->discrete.width = fse.max_width; 431 fsize->discrete.height = fse.max_height; 432 433 return 0; 434} 435 436static int cal_enum_input(struct file *file, void *priv, 437 struct v4l2_input *inp) 438{ 439 if (inp->index > 0) 440 return -EINVAL; 441 442 inp->type = V4L2_INPUT_TYPE_CAMERA; 443 sprintf(inp->name, "Camera %u", inp->index); 444 return 0; 445} 446 447static int cal_g_input(struct file *file, void *priv, unsigned int *i) 448{ 449 *i = 0; 450 return 0; 451} 452 453static int cal_s_input(struct file *file, void *priv, unsigned int i) 454{ 455 return i > 0 ? -EINVAL : 0; 456} 457 458/* timeperframe is arbitrary and continuous */ 459static int cal_enum_frameintervals(struct file *file, void *priv, 460 struct v4l2_frmivalenum *fival) 461{ 462 struct cal_ctx *ctx = video_drvdata(file); 463 const struct cal_fmt *fmt; 464 struct v4l2_subdev_frame_interval_enum fie = { 465 .index = fival->index, 466 .width = fival->width, 467 .height = fival->height, 468 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 469 }; 470 int ret; 471 472 fmt = find_format_by_pix(ctx, fival->pixel_format); 473 if (!fmt) 474 return -EINVAL; 475 476 fie.code = fmt->code; 477 ret = v4l2_subdev_call(ctx->phy->sensor, pad, enum_frame_interval, 478 NULL, &fie); 479 if (ret) 480 return ret; 481 fival->type = V4L2_FRMIVAL_TYPE_DISCRETE; 482 fival->discrete = fie.interval; 483 484 return 0; 485} 486 487static const struct v4l2_file_operations cal_fops = { 488 .owner = THIS_MODULE, 489 .open = v4l2_fh_open, 490 .release = vb2_fop_release, 491 .read = vb2_fop_read, 492 .poll = vb2_fop_poll, 493 .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */ 494 .mmap = vb2_fop_mmap, 495}; 496 497static const struct v4l2_ioctl_ops cal_ioctl_ops = { 498 .vidioc_querycap = cal_querycap, 499 .vidioc_enum_fmt_vid_cap = cal_enum_fmt_vid_cap, 500 .vidioc_g_fmt_vid_cap = cal_g_fmt_vid_cap, 501 .vidioc_try_fmt_vid_cap = cal_try_fmt_vid_cap, 502 .vidioc_s_fmt_vid_cap = cal_s_fmt_vid_cap, 503 .vidioc_enum_framesizes = cal_enum_framesizes, 504 .vidioc_reqbufs = vb2_ioctl_reqbufs, 505 .vidioc_create_bufs = vb2_ioctl_create_bufs, 506 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 507 .vidioc_querybuf = vb2_ioctl_querybuf, 508 .vidioc_qbuf = vb2_ioctl_qbuf, 509 .vidioc_dqbuf = vb2_ioctl_dqbuf, 510 .vidioc_expbuf = vb2_ioctl_expbuf, 511 .vidioc_enum_input = cal_enum_input, 512 .vidioc_g_input = cal_g_input, 513 .vidioc_s_input = cal_s_input, 514 .vidioc_enum_frameintervals = cal_enum_frameintervals, 515 .vidioc_streamon = vb2_ioctl_streamon, 516 .vidioc_streamoff = vb2_ioctl_streamoff, 517 .vidioc_log_status = v4l2_ctrl_log_status, 518 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, 519 .vidioc_unsubscribe_event = v4l2_event_unsubscribe, 520}; 521 522/* ------------------------------------------------------------------ 523 * videobuf2 Operations 524 * ------------------------------------------------------------------ 525 */ 526 527static int cal_queue_setup(struct vb2_queue *vq, 528 unsigned int *nbuffers, unsigned int *nplanes, 529 unsigned int sizes[], struct device *alloc_devs[]) 530{ 531 struct cal_ctx *ctx = vb2_get_drv_priv(vq); 532 unsigned int size = ctx->v_fmt.fmt.pix.sizeimage; 533 534 if (vq->num_buffers + *nbuffers < 3) 535 *nbuffers = 3 - vq->num_buffers; 536 537 if (*nplanes) { 538 if (sizes[0] < size) 539 return -EINVAL; 540 size = sizes[0]; 541 } 542 543 *nplanes = 1; 544 sizes[0] = size; 545 546 ctx_dbg(3, ctx, "nbuffers=%d, size=%d\n", *nbuffers, sizes[0]); 547 548 return 0; 549} 550 551static int cal_buffer_prepare(struct vb2_buffer *vb) 552{ 553 struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 554 struct cal_buffer *buf = container_of(vb, struct cal_buffer, 555 vb.vb2_buf); 556 unsigned long size; 557 558 if (WARN_ON(!ctx->fmt)) 559 return -EINVAL; 560 561 size = ctx->v_fmt.fmt.pix.sizeimage; 562 if (vb2_plane_size(vb, 0) < size) { 563 ctx_err(ctx, 564 "data will not fit into plane (%lu < %lu)\n", 565 vb2_plane_size(vb, 0), size); 566 return -EINVAL; 567 } 568 569 vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size); 570 return 0; 571} 572 573static void cal_buffer_queue(struct vb2_buffer *vb) 574{ 575 struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); 576 struct cal_buffer *buf = container_of(vb, struct cal_buffer, 577 vb.vb2_buf); 578 struct cal_dmaqueue *vidq = &ctx->vidq; 579 unsigned long flags; 580 581 /* recheck locking */ 582 spin_lock_irqsave(&ctx->slock, flags); 583 list_add_tail(&buf->list, &vidq->active); 584 spin_unlock_irqrestore(&ctx->slock, flags); 585} 586 587static int cal_start_streaming(struct vb2_queue *vq, unsigned int count) 588{ 589 struct cal_ctx *ctx = vb2_get_drv_priv(vq); 590 struct cal_dmaqueue *dma_q = &ctx->vidq; 591 struct cal_buffer *buf, *tmp; 592 unsigned long addr; 593 unsigned long flags; 594 int ret; 595 596 spin_lock_irqsave(&ctx->slock, flags); 597 if (list_empty(&dma_q->active)) { 598 spin_unlock_irqrestore(&ctx->slock, flags); 599 ctx_dbg(3, ctx, "buffer queue is empty\n"); 600 return -EIO; 601 } 602 603 buf = list_entry(dma_q->active.next, struct cal_buffer, list); 604 ctx->cur_frm = buf; 605 ctx->next_frm = buf; 606 list_del(&buf->list); 607 spin_unlock_irqrestore(&ctx->slock, flags); 608 609 addr = vb2_dma_contig_plane_dma_addr(&ctx->cur_frm->vb.vb2_buf, 0); 610 ctx->sequence = 0; 611 612 pm_runtime_get_sync(ctx->cal->dev); 613 614 cal_ctx_csi2_config(ctx); 615 cal_ctx_pix_proc_config(ctx); 616 cal_ctx_wr_dma_config(ctx, ctx->v_fmt.fmt.pix.bytesperline, 617 ctx->v_fmt.fmt.pix.height); 618 619 cal_camerarx_enable_irqs(ctx->phy); 620 621 ret = cal_camerarx_start(ctx->phy, ctx->fmt); 622 if (ret) 623 goto err; 624 625 cal_ctx_wr_dma_addr(ctx, addr); 626 cal_camerarx_ppi_enable(ctx->phy); 627 628 if (cal_debug >= 4) 629 cal_quickdump_regs(ctx->cal); 630 631 return 0; 632 633err: 634 spin_lock_irqsave(&ctx->slock, flags); 635 vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_QUEUED); 636 ctx->cur_frm = NULL; 637 ctx->next_frm = NULL; 638 list_for_each_entry_safe(buf, tmp, &dma_q->active, list) { 639 list_del(&buf->list); 640 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED); 641 } 642 spin_unlock_irqrestore(&ctx->slock, flags); 643 return ret; 644} 645 646static void cal_stop_streaming(struct vb2_queue *vq) 647{ 648 struct cal_ctx *ctx = vb2_get_drv_priv(vq); 649 struct cal_dmaqueue *dma_q = &ctx->vidq; 650 struct cal_buffer *buf, *tmp; 651 unsigned long timeout; 652 unsigned long flags; 653 bool dma_act; 654 655 cal_camerarx_ppi_disable(ctx->phy); 656 657 /* wait for stream and dma to finish */ 658 dma_act = true; 659 timeout = jiffies + msecs_to_jiffies(500); 660 while (dma_act && time_before(jiffies, timeout)) { 661 msleep(50); 662 663 spin_lock_irqsave(&ctx->slock, flags); 664 dma_act = ctx->dma_act; 665 spin_unlock_irqrestore(&ctx->slock, flags); 666 } 667 668 if (dma_act) 669 ctx_err(ctx, "failed to disable dma cleanly\n"); 670 671 cal_camerarx_disable_irqs(ctx->phy); 672 cal_camerarx_stop(ctx->phy); 673 674 /* Release all active buffers */ 675 spin_lock_irqsave(&ctx->slock, flags); 676 list_for_each_entry_safe(buf, tmp, &dma_q->active, list) { 677 list_del(&buf->list); 678 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 679 } 680 681 if (ctx->cur_frm == ctx->next_frm) { 682 vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR); 683 } else { 684 vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR); 685 vb2_buffer_done(&ctx->next_frm->vb.vb2_buf, 686 VB2_BUF_STATE_ERROR); 687 } 688 ctx->cur_frm = NULL; 689 ctx->next_frm = NULL; 690 spin_unlock_irqrestore(&ctx->slock, flags); 691 692 pm_runtime_put_sync(ctx->cal->dev); 693} 694 695static const struct vb2_ops cal_video_qops = { 696 .queue_setup = cal_queue_setup, 697 .buf_prepare = cal_buffer_prepare, 698 .buf_queue = cal_buffer_queue, 699 .start_streaming = cal_start_streaming, 700 .stop_streaming = cal_stop_streaming, 701 .wait_prepare = vb2_ops_wait_prepare, 702 .wait_finish = vb2_ops_wait_finish, 703}; 704 705/* ------------------------------------------------------------------ 706 * V4L2 Initialization and Registration 707 * ------------------------------------------------------------------ 708 */ 709 710static const struct video_device cal_videodev = { 711 .name = CAL_MODULE_NAME, 712 .fops = &cal_fops, 713 .ioctl_ops = &cal_ioctl_ops, 714 .minor = -1, 715 .release = video_device_release_empty, 716 .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING | 717 V4L2_CAP_READWRITE, 718}; 719 720static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx) 721{ 722 struct v4l2_subdev_mbus_code_enum mbus_code; 723 struct v4l2_mbus_framefmt mbus_fmt; 724 const struct cal_fmt *fmt; 725 unsigned int i, j, k; 726 int ret = 0; 727 728 /* Enumerate sub device formats and enable all matching local formats */ 729 ctx->active_fmt = devm_kcalloc(ctx->cal->dev, ARRAY_SIZE(cal_formats), 730 sizeof(*ctx->active_fmt), GFP_KERNEL); 731 ctx->num_active_fmt = 0; 732 733 for (j = 0, i = 0; ret != -EINVAL; ++j) { 734 735 memset(&mbus_code, 0, sizeof(mbus_code)); 736 mbus_code.index = j; 737 mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE; 738 ret = v4l2_subdev_call(ctx->phy->sensor, pad, enum_mbus_code, 739 NULL, &mbus_code); 740 if (ret) 741 continue; 742 743 ctx_dbg(2, ctx, 744 "subdev %s: code: %04x idx: %u\n", 745 ctx->phy->sensor->name, mbus_code.code, j); 746 747 for (k = 0; k < ARRAY_SIZE(cal_formats); k++) { 748 const struct cal_fmt *fmt = &cal_formats[k]; 749 750 if (mbus_code.code == fmt->code) { 751 ctx->active_fmt[i] = fmt; 752 ctx_dbg(2, ctx, 753 "matched fourcc: %s: code: %04x idx: %u\n", 754 fourcc_to_str(fmt->fourcc), 755 fmt->code, i); 756 ctx->num_active_fmt = ++i; 757 } 758 } 759 } 760 761 if (i == 0) { 762 ctx_err(ctx, "No suitable format reported by subdev %s\n", 763 ctx->phy->sensor->name); 764 return -EINVAL; 765 } 766 767 ret = __subdev_get_format(ctx, &mbus_fmt); 768 if (ret) 769 return ret; 770 771 fmt = find_format_by_code(ctx, mbus_fmt.code); 772 if (!fmt) { 773 ctx_dbg(3, ctx, "mbus code format (0x%08x) not found.\n", 774 mbus_fmt.code); 775 return -EINVAL; 776 } 777 778 /* Save current subdev format */ 779 v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt); 780 ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 781 ctx->v_fmt.fmt.pix.pixelformat = fmt->fourcc; 782 cal_calc_format_size(ctx, fmt, &ctx->v_fmt); 783 ctx->fmt = fmt; 784 ctx->m_fmt = mbus_fmt; 785 786 return 0; 787} 788 789int cal_ctx_v4l2_register(struct cal_ctx *ctx) 790{ 791 struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler; 792 struct video_device *vfd = &ctx->vdev; 793 int ret; 794 795 ret = cal_ctx_v4l2_init_formats(ctx); 796 if (ret) 797 return ret; 798 799 ret = v4l2_ctrl_add_handler(hdl, ctx->phy->sensor->ctrl_handler, NULL, 800 true); 801 if (ret < 0) { 802 ctx_err(ctx, "Failed to add sensor ctrl handler\n"); 803 return ret; 804 } 805 806 ret = video_register_device(vfd, VFL_TYPE_VIDEO, cal_video_nr); 807 if (ret < 0) { 808 ctx_err(ctx, "Failed to register video device\n"); 809 return ret; 810 } 811 812 ctx_info(ctx, "V4L2 device registered as %s\n", 813 video_device_node_name(vfd)); 814 815 return 0; 816} 817 818void cal_ctx_v4l2_unregister(struct cal_ctx *ctx) 819{ 820 ctx_dbg(1, ctx, "unregistering %s\n", 821 video_device_node_name(&ctx->vdev)); 822 823 video_unregister_device(&ctx->vdev); 824} 825 826int cal_ctx_v4l2_init(struct cal_ctx *ctx) 827{ 828 struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler; 829 struct video_device *vfd = &ctx->vdev; 830 struct vb2_queue *q = &ctx->vb_vidq; 831 int ret; 832 833 INIT_LIST_HEAD(&ctx->vidq.active); 834 spin_lock_init(&ctx->slock); 835 mutex_init(&ctx->mutex); 836 837 /* Initialize the vb2 queue. */ 838 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 839 q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ; 840 q->drv_priv = ctx; 841 q->buf_struct_size = sizeof(struct cal_buffer); 842 q->ops = &cal_video_qops; 843 q->mem_ops = &vb2_dma_contig_memops; 844 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 845 q->lock = &ctx->mutex; 846 q->min_buffers_needed = 3; 847 q->dev = ctx->cal->dev; 848 849 ret = vb2_queue_init(q); 850 if (ret) 851 return ret; 852 853 /* Initialize the video device and media entity. */ 854 *vfd = cal_videodev; 855 vfd->v4l2_dev = &ctx->cal->v4l2_dev; 856 vfd->queue = q; 857 snprintf(vfd->name, sizeof(vfd->name), "CAL output %u", ctx->index); 858 vfd->lock = &ctx->mutex; 859 video_set_drvdata(vfd, ctx); 860 861 ctx->pad.flags = MEDIA_PAD_FL_SINK; 862 ret = media_entity_pads_init(&vfd->entity, 1, &ctx->pad); 863 if (ret < 0) 864 return ret; 865 866 /* Initialize the control handler. */ 867 ret = v4l2_ctrl_handler_init(hdl, 11); 868 if (ret < 0) { 869 ctx_err(ctx, "Failed to init ctrl handler\n"); 870 goto error; 871 } 872 873 vfd->ctrl_handler = hdl; 874 875 return 0; 876 877error: 878 media_entity_cleanup(&vfd->entity); 879 return ret; 880} 881 882void cal_ctx_v4l2_cleanup(struct cal_ctx *ctx) 883{ 884 v4l2_ctrl_handler_free(&ctx->ctrl_handler); 885 media_entity_cleanup(&ctx->vdev.entity); 886} 887