drivers/staging/media/rpivid/rpivid_video.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Raspberry Pi HEVC driver
   4  *
   5  * Copyright (C) 2020 Raspberry Pi (Trading) Ltd
   6  *
   7  * Based on the Cedrus VPU driver, that is:
   8  *
   9  * Copyright (C) 2016 Florent Revest <florent.revest@free-electrons.com>
  10  * Copyright (C) 2018 Paul Kocialkowski <paul.kocialkowski@bootlin.com>
  11  * Copyright (C) 2018 Bootlin
  12  */
  13
  14 #include <media/videobuf2-dma-contig.h>
  15 #include <media/v4l2-device.h>
  16 #include <media/v4l2-ioctl.h>
  17 #include <media/v4l2-event.h>
  18 #include <media/v4l2-mem2mem.h>
  19
  20 #include "rpivid.h"
  21 #include "rpivid_hw.h"
  22 #include "rpivid_video.h"
  23 #include "rpivid_dec.h"
  24
  25 #define RPIVID_DECODE_SRC       BIT(0)
  26 #define RPIVID_DECODE_DST       BIT(1)
  27
  28 #define RPIVID_MIN_WIDTH        16U
  29 #define RPIVID_MIN_HEIGHT       16U
  30 #define RPIVID_DEFAULT_WIDTH    1920U
  31 #define RPIVID_DEFAULT_HEIGHT   1088U
  32 #define RPIVID_MAX_WIDTH        4096U
  33 #define RPIVID_MAX_HEIGHT       4096U
  34
  35 static inline struct rpivid_ctx *rpivid_file2ctx(struct file *file)
  36 {
  37         return container_of(file->private_data, struct rpivid_ctx, fh);
  38 }
  39
  40 /* constrain x to y,y*2 */
  41 static inline unsigned int constrain2x(unsigned int x, unsigned int y)
  42 {
  43         return (x < y) ?
  44                         y :
  45                         (x > y * 2) ? y : x;
  46 }
  47
  48 size_t rpivid_round_up_size(const size_t x)
  49 {
  50         /* Admit no size < 256 */
  51         const unsigned int n = x < 256 ? 8 : ilog2(x);
  52
  53         return x >= (3 << n) ? 4 << n : (3 << n);
  54 }
  55
  56 size_t rpivid_bit_buf_size(unsigned int w, unsigned int h, unsigned int bits_minus8)
  57 {
  58         const size_t wxh = w * h;
  59         size_t bits_alloc;
  60
  61         /* Annex A gives a min compression of 2 @ lvl 3.1
  62          * (wxh <= 983040) and min 4 thereafter but avoid
  63          * the odity of 983041 having a lower limit than
  64          * 983040.
  65          * Multiply by 3/2 for 4:2:0
  66          */
  67         bits_alloc = wxh < 983040 ? wxh * 3 / 4 :
  68                 wxh < 983040 * 2 ? 983040 * 3 / 4 :
  69                 wxh * 3 / 8;
  70         /* Allow for bit depth */
  71         bits_alloc += (bits_alloc * bits_minus8) / 8;
  72         return rpivid_round_up_size(bits_alloc);
  73 }
  74
  75 void rpivid_prepare_src_format(struct v4l2_pix_format_mplane *pix_fmt)
  76 {
  77         size_t size;
  78         u32 w;
  79         u32 h;
  80
  81         w = pix_fmt->width;
  82         h = pix_fmt->height;
  83         if (!w || !h) {
  84                 w = RPIVID_DEFAULT_WIDTH;
  85                 h = RPIVID_DEFAULT_HEIGHT;
  86         }
  87         if (w > RPIVID_MAX_WIDTH)
  88                 w = RPIVID_MAX_WIDTH;
  89         if (h > RPIVID_MAX_HEIGHT)
  90                 h = RPIVID_MAX_HEIGHT;
  91
  92         if (!pix_fmt->plane_fmt[0].sizeimage ||
  93             pix_fmt->plane_fmt[0].sizeimage > SZ_32M) {
  94                 /* Unspecified or way too big - pick max for size */
  95                 size = rpivid_bit_buf_size(w, h, 2);
  96         }
  97         /* Set a minimum */
  98         size = max_t(u32, SZ_4K, pix_fmt->plane_fmt[0].sizeimage);
  99
 100         pix_fmt->pixelformat = V4L2_PIX_FMT_HEVC_SLICE;
 101         pix_fmt->width = w;
 102         pix_fmt->height = h;
 103         pix_fmt->num_planes = 1;
 104         pix_fmt->field = V4L2_FIELD_NONE;
 105         /* Zero bytes per line for encoded source. */
 106         pix_fmt->plane_fmt[0].bytesperline = 0;
 107         pix_fmt->plane_fmt[0].sizeimage = size;
 108 }
 109
 110 /* Take any pix_format and make it valid */
 111 static void rpivid_prepare_dst_format(struct v4l2_pix_format_mplane *pix_fmt)
 112 {
 113         unsigned int width = pix_fmt->width;
 114         unsigned int height = pix_fmt->height;
 115         unsigned int sizeimage = pix_fmt->plane_fmt[0].sizeimage;
 116         unsigned int bytesperline = pix_fmt->plane_fmt[0].bytesperline;
 117
 118         if (!width)
 119                 width = RPIVID_DEFAULT_WIDTH;
 120         if (width > RPIVID_MAX_WIDTH)
 121                 width = RPIVID_MAX_WIDTH;
 122         if (!height)
 123                 height = RPIVID_DEFAULT_HEIGHT;
 124         if (height > RPIVID_MAX_HEIGHT)
 125                 height = RPIVID_MAX_HEIGHT;
 126
 127         /* For column formats set bytesperline to column height (stride2) */
 128         switch (pix_fmt->pixelformat) {
 129         default:
 130                 pix_fmt->pixelformat = V4L2_PIX_FMT_NV12_COL128;
 131                 fallthrough;
 132         case V4L2_PIX_FMT_NV12_COL128:
 133                 /* Width rounds up to columns */
 134                 width = ALIGN(width, 128);
 135
 136                 /* 16 aligned height - not sure we even need that */
 137                 height = ALIGN(height, 16);
 138                 /* column height
 139                  * Accept suggested shape if at least min & < 2 * min
 140                  */
 141                 bytesperline = constrain2x(bytesperline, height * 3 / 2);
 142
 143                 /* image size
 144                  * Again allow plausible variation in case added padding is
 145                  * required
 146                  */
 147                 sizeimage = constrain2x(sizeimage, bytesperline * width);
 148                 break;
 149
 150         case V4L2_PIX_FMT_NV12_10_COL128:
 151                 /* width in pixels (3 pels = 4 bytes) rounded to 128 byte
 152                  * columns
 153                  */
 154                 width = ALIGN(((width + 2) / 3), 32) * 3;
 155
 156                 /* 16-aligned height. */
 157                 height = ALIGN(height, 16);
 158
 159                 /* column height
 160                  * Accept suggested shape if at least min & < 2 * min
 161                  */
 162                 bytesperline = constrain2x(bytesperline, height * 3 / 2);
 163
 164                 /* image size
 165                  * Again allow plausible variation in case added padding is
 166                  * required
 167                  */
 168                 sizeimage = constrain2x(sizeimage,
 169                                         bytesperline * width * 4 / 3);
 170                 break;
 171         }
 172
 173         pix_fmt->width = width;
 174         pix_fmt->height = height;
 175
 176         pix_fmt->field = V4L2_FIELD_NONE;
 177         pix_fmt->plane_fmt[0].bytesperline = bytesperline;
 178         pix_fmt->plane_fmt[0].sizeimage = sizeimage;
 179         pix_fmt->num_planes = 1;
 180 }
 181
 182 static int rpivid_querycap(struct file *file, void *priv,
 183                            struct v4l2_capability *cap)
 184 {
 185         strscpy(cap->driver, RPIVID_NAME, sizeof(cap->driver));
 186         strscpy(cap->card, RPIVID_NAME, sizeof(cap->card));
 187         snprintf(cap->bus_info, sizeof(cap->bus_info),
 188                  "platform:%s", RPIVID_NAME);
 189
 190         return 0;
 191 }
 192
 193 static int rpivid_enum_fmt_vid_out(struct file *file, void *priv,
 194                                    struct v4l2_fmtdesc *f)
 195 {
 196         // Input formats
 197
 198         // H.265 Slice only currently
 199         if (f->index == 0) {
 200                 f->pixelformat = V4L2_PIX_FMT_HEVC_SLICE;
 201                 return 0;
 202         }
 203
 204         return -EINVAL;
 205 }
 206
 207 static int rpivid_hevc_validate_sps(const struct v4l2_ctrl_hevc_sps * const sps)
 208 {
 209         const unsigned int ctb_log2_size_y =
 210                         sps->log2_min_luma_coding_block_size_minus3 + 3 +
 211                         sps->log2_diff_max_min_luma_coding_block_size;
 212         const unsigned int min_tb_log2_size_y =
 213                         sps->log2_min_luma_transform_block_size_minus2 + 2;
 214         const unsigned int max_tb_log2_size_y = min_tb_log2_size_y +
 215                         sps->log2_diff_max_min_luma_transform_block_size;
 216
 217         /* Local limitations */
 218         if (sps->pic_width_in_luma_samples < 32 ||
 219             sps->pic_width_in_luma_samples > 4096)
 220                 return 0;
 221         if (sps->pic_height_in_luma_samples < 32 ||
 222             sps->pic_height_in_luma_samples > 4096)
 223                 return 0;
 224         if (!(sps->bit_depth_luma_minus8 == 0 ||
 225               sps->bit_depth_luma_minus8 == 2))
 226                 return 0;
 227         if (sps->bit_depth_luma_minus8 != sps->bit_depth_chroma_minus8)
 228                 return 0;
 229         if (sps->chroma_format_idc != 1)
 230                 return 0;
 231
 232         /*  Limits from H.265 7.4.3.2.1 */
 233         if (sps->log2_max_pic_order_cnt_lsb_minus4 > 12)
 234                 return 0;
 235         if (sps->sps_max_dec_pic_buffering_minus1 > 15)
 236                 return 0;
 237         if (sps->sps_max_num_reorder_pics >
 238                                 sps->sps_max_dec_pic_buffering_minus1)
 239                 return 0;
 240         if (ctb_log2_size_y > 6)
 241                 return 0;
 242         if (max_tb_log2_size_y > 5)
 243                 return 0;
 244         if (max_tb_log2_size_y > ctb_log2_size_y)
 245                 return 0;
 246         if (sps->max_transform_hierarchy_depth_inter >
 247                                 (ctb_log2_size_y - min_tb_log2_size_y))
 248                 return 0;
 249         if (sps->max_transform_hierarchy_depth_intra >
 250                                 (ctb_log2_size_y - min_tb_log2_size_y))
 251                 return 0;
 252         /* Check pcm stuff */
 253         if (sps->num_short_term_ref_pic_sets > 64)
 254                 return 0;
 255         if (sps->num_long_term_ref_pics_sps > 32)
 256                 return 0;
 257         return 1;
 258 }
 259
 260 static inline int is_sps_set(const struct v4l2_ctrl_hevc_sps * const sps)
 261 {
 262         return sps && sps->pic_width_in_luma_samples != 0;
 263 }
 264
 265 static u32 pixelformat_from_sps(const struct v4l2_ctrl_hevc_sps * const sps,
 266                                 const int index)
 267 {
 268         u32 pf = 0;
 269
 270         if (!is_sps_set(sps) || !rpivid_hevc_validate_sps(sps)) {
 271                 /* Treat this as an error? For now return both */
 272                 if (index == 0)
 273                         pf = V4L2_PIX_FMT_NV12_COL128;
 274                 else if (index == 1)
 275                         pf = V4L2_PIX_FMT_NV12_10_COL128;
 276         } else if (index == 0) {
 277                 if (sps->bit_depth_luma_minus8 == 0)
 278                         pf = V4L2_PIX_FMT_NV12_COL128;
 279                 else if (sps->bit_depth_luma_minus8 == 2)
 280                         pf = V4L2_PIX_FMT_NV12_10_COL128;
 281         }
 282
 283         return pf;
 284 }
 285
 286 static struct v4l2_pix_format_mplane
 287 rpivid_hevc_default_dst_fmt(struct rpivid_ctx * const ctx)
 288 {
 289         const struct v4l2_ctrl_hevc_sps * const sps =
 290                 rpivid_find_control_data(ctx, V4L2_CID_MPEG_VIDEO_HEVC_SPS);
 291         struct v4l2_pix_format_mplane pix_fmt;
 292
 293         memset(&pix_fmt, 0, sizeof(pix_fmt));
 294         if (is_sps_set(sps)) {
 295                 pix_fmt.width = sps->pic_width_in_luma_samples;
 296                 pix_fmt.height = sps->pic_height_in_luma_samples;
 297                 pix_fmt.pixelformat = pixelformat_from_sps(sps, 0);
 298         }
 299
 300         rpivid_prepare_dst_format(&pix_fmt);
 301         return pix_fmt;
 302 }
 303
 304 static u32 rpivid_hevc_get_dst_pixelformat(struct rpivid_ctx * const ctx,
 305                                            const int index)
 306 {
 307         const struct v4l2_ctrl_hevc_sps * const sps =
 308                 rpivid_find_control_data(ctx, V4L2_CID_MPEG_VIDEO_HEVC_SPS);
 309
 310         return pixelformat_from_sps(sps, index);
 311 }
 312
 313 static int rpivid_enum_fmt_vid_cap(struct file *file, void *priv,
 314                                    struct v4l2_fmtdesc *f)
 315 {
 316         struct rpivid_ctx * const ctx = rpivid_file2ctx(file);
 317
 318         const u32 pf = rpivid_hevc_get_dst_pixelformat(ctx, f->index);
 319
 320         if (pf == 0)
 321                 return -EINVAL;
 322
 323         f->pixelformat = pf;
 324         return 0;
 325 }
 326
 327 /*
 328  * get dst format - sets it to default if otherwise unset
 329  * returns a pointer to the struct as a convienience
 330  */
 331 static struct v4l2_pix_format_mplane *get_dst_fmt(struct rpivid_ctx *const ctx)
 332 {
 333         if (!ctx->dst_fmt_set)
 334                 ctx->dst_fmt = rpivid_hevc_default_dst_fmt(ctx);
 335         return &ctx->dst_fmt;
 336 }
 337
 338 static int rpivid_g_fmt_vid_cap(struct file *file, void *priv,
 339                                 struct v4l2_format *f)
 340 {
 341         struct rpivid_ctx *ctx = rpivid_file2ctx(file);
 342
 343         f->fmt.pix_mp = *get_dst_fmt(ctx);
 344         return 0;
 345 }
 346
 347 static int rpivid_g_fmt_vid_out(struct file *file, void *priv,
 348                                 struct v4l2_format *f)
 349 {
 350         struct rpivid_ctx *ctx = rpivid_file2ctx(file);
 351
 352         f->fmt.pix_mp = ctx->src_fmt;
 353         return 0;
 354 }
 355
 356 static inline void copy_color(struct v4l2_pix_format_mplane *d,
 357                               const struct v4l2_pix_format_mplane *s)
 358 {
 359         d->colorspace   = s->colorspace;
 360         d->xfer_func    = s->xfer_func;
 361         d->ycbcr_enc    = s->ycbcr_enc;
 362         d->quantization = s->quantization;
 363 }
 364
 365 static int rpivid_try_fmt_vid_cap(struct file *file, void *priv,
 366                                   struct v4l2_format *f)
 367 {
 368         struct rpivid_ctx *ctx = rpivid_file2ctx(file);
 369         const struct v4l2_ctrl_hevc_sps * const sps =
 370                 rpivid_find_control_data(ctx, V4L2_CID_MPEG_VIDEO_HEVC_SPS);
 371         u32 pixelformat;
 372         int i;
 373
 374         for (i = 0; (pixelformat = pixelformat_from_sps(sps, i)) != 0; i++) {
 375                 if (f->fmt.pix_mp.pixelformat == pixelformat)
 376                         break;
 377         }
 378
 379         // We don't have any way of finding out colourspace so believe
 380         // anything we are told - take anything set in src as a default
 381         if (f->fmt.pix_mp.colorspace == V4L2_COLORSPACE_DEFAULT)
 382                 copy_color(&f->fmt.pix_mp, &ctx->src_fmt);
 383
 384         f->fmt.pix_mp.pixelformat = pixelformat;
 385         rpivid_prepare_dst_format(&f->fmt.pix_mp);
 386         return 0;
 387 }
 388
 389 static int rpivid_try_fmt_vid_out(struct file *file, void *priv,
 390                                   struct v4l2_format *f)
 391 {
 392         rpivid_prepare_src_format(&f->fmt.pix_mp);
 393         return 0;
 394 }
 395
 396 static int rpivid_s_fmt_vid_cap(struct file *file, void *priv,
 397                                 struct v4l2_format *f)
 398 {
 399         struct rpivid_ctx *ctx = rpivid_file2ctx(file);
 400         struct vb2_queue *vq;
 401         int ret;
 402
 403         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 404         if (vb2_is_busy(vq))
 405                 return -EBUSY;
 406
 407         ret = rpivid_try_fmt_vid_cap(file, priv, f);
 408         if (ret)
 409                 return ret;
 410
 411         ctx->dst_fmt = f->fmt.pix_mp;
 412         ctx->dst_fmt_set = 1;
 413
 414         return 0;
 415 }
 416
 417 static int rpivid_s_fmt_vid_out(struct file *file, void *priv,
 418                                 struct v4l2_format *f)
 419 {
 420         struct rpivid_ctx *ctx = rpivid_file2ctx(file);
 421         struct vb2_queue *vq;
 422         int ret;
 423
 424         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 425         if (vb2_is_busy(vq))
 426                 return -EBUSY;
 427
 428         ret = rpivid_try_fmt_vid_out(file, priv, f);
 429         if (ret)
 430                 return ret;
 431
 432         ctx->src_fmt = f->fmt.pix_mp;
 433         ctx->dst_fmt_set = 0;  // Setting src invalidates dst
 434
 435         vq->subsystem_flags |=
 436                 VB2_V4L2_FL_SUPPORTS_M2M_HOLD_CAPTURE_BUF;
 437
 438         /* Propagate colorspace information to capture. */
 439         copy_color(&ctx->dst_fmt, &f->fmt.pix_mp);
 440         return 0;
 441 }
 442
 443 const struct v4l2_ioctl_ops rpivid_ioctl_ops = {
 444         .vidioc_querycap                = rpivid_querycap,
 445
 446         .vidioc_enum_fmt_vid_cap        = rpivid_enum_fmt_vid_cap,
 447         .vidioc_g_fmt_vid_cap_mplane    = rpivid_g_fmt_vid_cap,
 448         .vidioc_try_fmt_vid_cap_mplane  = rpivid_try_fmt_vid_cap,
 449         .vidioc_s_fmt_vid_cap_mplane    = rpivid_s_fmt_vid_cap,
 450
 451         .vidioc_enum_fmt_vid_out        = rpivid_enum_fmt_vid_out,
 452         .vidioc_g_fmt_vid_out_mplane    = rpivid_g_fmt_vid_out,
 453         .vidioc_try_fmt_vid_out_mplane  = rpivid_try_fmt_vid_out,
 454         .vidioc_s_fmt_vid_out_mplane    = rpivid_s_fmt_vid_out,
 455
 456         .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
 457         .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
 458         .vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
 459         .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
 460         .vidioc_prepare_buf             = v4l2_m2m_ioctl_prepare_buf,
 461         .vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
 462         .vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
 463
 464         .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
 465         .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
 466
 467         .vidioc_try_decoder_cmd         = v4l2_m2m_ioctl_stateless_try_decoder_cmd,
 468         .vidioc_decoder_cmd             = v4l2_m2m_ioctl_stateless_decoder_cmd,
 469
 470         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
 471         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
 472 };
 473
 474 static int rpivid_queue_setup(struct vb2_queue *vq, unsigned int *nbufs,
 475                               unsigned int *nplanes, unsigned int sizes[],
 476                               struct device *alloc_devs[])
 477 {
 478         struct rpivid_ctx *ctx = vb2_get_drv_priv(vq);
 479         struct v4l2_pix_format_mplane *pix_fmt;
 480
 481         if (V4L2_TYPE_IS_OUTPUT(vq->type))
 482                 pix_fmt = &ctx->src_fmt;
 483         else
 484                 pix_fmt = get_dst_fmt(ctx);
 485
 486         if (*nplanes) {
 487                 if (sizes[0] < pix_fmt->plane_fmt[0].sizeimage)
 488                         return -EINVAL;
 489         } else {
 490                 sizes[0] = pix_fmt->plane_fmt[0].sizeimage;
 491                 *nplanes = 1;
 492         }
 493
 494         return 0;
 495 }
 496
 497 static void rpivid_queue_cleanup(struct vb2_queue *vq, u32 state)
 498 {
 499         struct rpivid_ctx *ctx = vb2_get_drv_priv(vq);
 500         struct vb2_v4l2_buffer *vbuf;
 501
 502         for (;;) {
 503                 if (V4L2_TYPE_IS_OUTPUT(vq->type))
 504                         vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 505                 else
 506                         vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 507
 508                 if (!vbuf)
 509                         return;
 510
 511                 v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req,
 512                                            &ctx->hdl);
 513                 v4l2_m2m_buf_done(vbuf, state);
 514         }
 515 }
 516
 517 static int rpivid_buf_out_validate(struct vb2_buffer *vb)
 518 {
 519         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 520
 521         vbuf->field = V4L2_FIELD_NONE;
 522         return 0;
 523 }
 524
 525 static int rpivid_buf_prepare(struct vb2_buffer *vb)
 526 {
 527         struct vb2_queue *vq = vb->vb2_queue;
 528         struct rpivid_ctx *ctx = vb2_get_drv_priv(vq);
 529         struct v4l2_pix_format_mplane *pix_fmt;
 530
 531         if (V4L2_TYPE_IS_OUTPUT(vq->type))
 532                 pix_fmt = &ctx->src_fmt;
 533         else
 534                 pix_fmt = &ctx->dst_fmt;
 535
 536         if (vb2_plane_size(vb, 0) < pix_fmt->plane_fmt[0].sizeimage)
 537                 return -EINVAL;
 538
 539         vb2_set_plane_payload(vb, 0, pix_fmt->plane_fmt[0].sizeimage);
 540
 541         return 0;
 542 }
 543
 544 /* Only stops the clock if streaom off on both output & capture */
 545 static void stop_clock(struct rpivid_dev *dev, struct rpivid_ctx *ctx)
 546 {
 547         if (ctx->src_stream_on ||
 548             ctx->dst_stream_on)
 549                 return;
 550
 551         clk_set_min_rate(dev->clock, 0);
 552         clk_disable_unprepare(dev->clock);
 553 }
 554
 555 /* Always starts the clock if it isn't already on this ctx */
 556 static int start_clock(struct rpivid_dev *dev, struct rpivid_ctx *ctx)
 557 {
 558         long max_hevc_clock;
 559         int rv;
 560
 561         max_hevc_clock = clk_get_max_rate(dev->clock);
 562
 563         rv = clk_set_min_rate(dev->clock, max_hevc_clock);
 564         if (rv) {
 565                 dev_err(dev->dev, "Failed to set clock rate\n");
 566                 return rv;
 567         }
 568
 569         rv = clk_prepare_enable(dev->clock);
 570         if (rv) {
 571                 dev_err(dev->dev, "Failed to enable clock\n");
 572                 return rv;
 573         }
 574
 575         return 0;
 576 }
 577
 578 static int rpivid_start_streaming(struct vb2_queue *vq, unsigned int count)
 579 {
 580         struct rpivid_ctx *ctx = vb2_get_drv_priv(vq);
 581         struct rpivid_dev *dev = ctx->dev;
 582         int ret = 0;
 583
 584         if (!V4L2_TYPE_IS_OUTPUT(vq->type)) {
 585                 ctx->dst_stream_on = 1;
 586                 goto ok;
 587         }
 588
 589         if (ctx->src_fmt.pixelformat != V4L2_PIX_FMT_HEVC_SLICE) {
 590                 ret = -EINVAL;
 591                 goto fail_cleanup;
 592         }
 593
 594         if (ctx->src_stream_on)
 595                 goto ok;
 596
 597         ret = start_clock(dev, ctx);
 598         if (ret)
 599                 goto fail_cleanup;
 600
 601         if (dev->dec_ops->start)
 602                 ret = dev->dec_ops->start(ctx);
 603         if (ret)
 604                 goto fail_stop_clock;
 605
 606         ctx->src_stream_on = 1;
 607 ok:
 608         return 0;
 609
 610 fail_stop_clock:
 611         stop_clock(dev, ctx);
 612 fail_cleanup:
 613         v4l2_err(&dev->v4l2_dev, "%s: qtype=%d: FAIL\n", __func__, vq->type);
 614         rpivid_queue_cleanup(vq, VB2_BUF_STATE_QUEUED);
 615         return ret;
 616 }
 617
 618 static void rpivid_stop_streaming(struct vb2_queue *vq)
 619 {
 620         struct rpivid_ctx *ctx = vb2_get_drv_priv(vq);
 621         struct rpivid_dev *dev = ctx->dev;
 622
 623         if (V4L2_TYPE_IS_OUTPUT(vq->type)) {
 624                 ctx->src_stream_on = 0;
 625                 if (dev->dec_ops->stop)
 626                         dev->dec_ops->stop(ctx);
 627         } else {
 628                 ctx->dst_stream_on = 0;
 629         }
 630
 631         rpivid_queue_cleanup(vq, VB2_BUF_STATE_ERROR);
 632
 633         vb2_wait_for_all_buffers(vq);
 634
 635         stop_clock(dev, ctx);
 636 }
 637
 638 static void rpivid_buf_queue(struct vb2_buffer *vb)
 639 {
 640         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 641         struct rpivid_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 642
 643         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
 644 }
 645
 646 static void rpivid_buf_request_complete(struct vb2_buffer *vb)
 647 {
 648         struct rpivid_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 649
 650         v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl);
 651 }
 652
 653 static struct vb2_ops rpivid_qops = {
 654         .queue_setup            = rpivid_queue_setup,
 655         .buf_prepare            = rpivid_buf_prepare,
 656         .buf_queue              = rpivid_buf_queue,
 657         .buf_out_validate       = rpivid_buf_out_validate,
 658         .buf_request_complete   = rpivid_buf_request_complete,
 659         .start_streaming        = rpivid_start_streaming,
 660         .stop_streaming         = rpivid_stop_streaming,
 661         .wait_prepare           = vb2_ops_wait_prepare,
 662         .wait_finish            = vb2_ops_wait_finish,
 663 };
 664
 665 int rpivid_queue_init(void *priv, struct vb2_queue *src_vq,
 666                       struct vb2_queue *dst_vq)
 667 {
 668         struct rpivid_ctx *ctx = priv;
 669         int ret;
 670
 671         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
 672         src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
 673         src_vq->drv_priv = ctx;
 674         src_vq->buf_struct_size = sizeof(struct rpivid_buffer);
 675         src_vq->ops = &rpivid_qops;
 676         src_vq->mem_ops = &vb2_dma_contig_memops;
 677         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 678         src_vq->lock = &ctx->ctx_mutex;
 679         src_vq->dev = ctx->dev->dev;
 680         src_vq->supports_requests = true;
 681         src_vq->requires_requests = true;
 682
 683         ret = vb2_queue_init(src_vq);
 684         if (ret)
 685                 return ret;
 686
 687         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 688         dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
 689         dst_vq->drv_priv = ctx;
 690         dst_vq->buf_struct_size = sizeof(struct rpivid_buffer);
 691         dst_vq->min_buffers_needed = 1;
 692         dst_vq->ops = &rpivid_qops;
 693         dst_vq->mem_ops = &vb2_dma_contig_memops;
 694         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 695         dst_vq->lock = &ctx->ctx_mutex;
 696         dst_vq->dev = ctx->dev->dev;
 697
 698         return vb2_queue_init(dst_vq);
 699 }