drivers/media/platform/ti-vpe/cal-video.c

   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
  34 static 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) */
 131 static 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
 149 static 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
 164 static 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
 179 static 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
 192 static 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
 208 static 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
 230 static 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
 251 static 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
 285 static 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
 295 static 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
 354 static 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
 399 static 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
 436 static 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
 447 static int cal_g_input(struct file *file, void *priv, unsigned int *i)
 448 {
 449         *i = 0;
 450         return 0;
 451 }
 452
 453 static 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 */
 459 static 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
 487 static 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
 497 static 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
 527 static 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
 551 static 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
 573 static 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
 587 static 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
 633 err:
 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
 646 static 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
 695 static 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
 710 static 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
 720 static 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
 789 int 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
 818 void 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
 826 int 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
 877 error:
 878         media_entity_cleanup(&vfd->entity);
 879         return ret;
 880 }
 881
 882 void 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 }