target/linux/bcm27xx/patches-5.4/950-0177-staging-vc04_services-Add-a-V4L2-M2M-codec-driver.patch

   1 From 6b2e734af2943dbf31bafb4c4c2fb588eec8059f Mon Sep 17 00:00:00 2001
   2 From: Dave Stevenson <dave.stevenson@raspberrypi.org>
   3 Date: Tue, 25 Sep 2018 14:53:49 +0100
   4 Subject: [PATCH] staging: vc04_services: Add a V4L2 M2M codec driver
   5
   6 This adds a V4L2 memory to memory device that wraps the MMAL
   7 video decode and video_encode components for H264 and MJPEG encode
   8 and decode, MPEG4, H263, and VP8 decode (and MPEG2 decode
   9 if the appropriate licence has been purchased).
  10
  11 Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.org>
  12 ---
  13  drivers/staging/vc04_services/Kconfig         |    1 +
  14  drivers/staging/vc04_services/Makefile        |    9 +-
  15  .../vc04_services/bcm2835-codec/Kconfig       |   11 +
  16  .../vc04_services/bcm2835-codec/Makefile      |    8 +
  17  .../staging/vc04_services/bcm2835-codec/TODO  |   24 +
  18  .../bcm2835-codec/bcm2835-v4l2-codec.c        | 2359 +++++++++++++++++
  19  6 files changed, 2408 insertions(+), 4 deletions(-)
  20  create mode 100644 drivers/staging/vc04_services/bcm2835-codec/Kconfig
  21  create mode 100644 drivers/staging/vc04_services/bcm2835-codec/Makefile
  22  create mode 100644 drivers/staging/vc04_services/bcm2835-codec/TODO
  23  create mode 100644 drivers/staging/vc04_services/bcm2835-codec/bcm2835-v4l2-codec.c
  24
  25 --- a/drivers/staging/vc04_services/Kconfig
  26 +++ b/drivers/staging/vc04_services/Kconfig
  27 @@ -24,6 +24,7 @@ source "drivers/staging/vc04_services/bc
  28  source "drivers/staging/vc04_services/bcm2835-camera/Kconfig"
  29  source "drivers/staging/vc04_services/vchiq-mmal/Kconfig"
  30  source "drivers/staging/vc04_services/vc-sm-cma/Kconfig"
  31 +source "drivers/staging/vc04_services/bcm2835-codec/Kconfig"
  32
  33  endif
  34
  35 --- a/drivers/staging/vc04_services/Makefile
  36 +++ b/drivers/staging/vc04_services/Makefile
  37 @@ -10,10 +10,11 @@ vchiq-objs := \
  38     interface/vchiq_arm/vchiq_util.o \
  39     interface/vchiq_arm/vchiq_connected.o \
  40
  41 -obj-$(CONFIG_SND_BCM2835)      += bcm2835-audio/
  42 -obj-$(CONFIG_VIDEO_BCM2835)    += bcm2835-camera/
  43 -obj-$(CONFIG_BCM2835_VCHIQ_MMAL) += vchiq-mmal/
  44 -obj-$(CONFIG_BCM_VC_SM_CMA)    += vc-sm-cma/
  45 +obj-$(CONFIG_SND_BCM2835)              += bcm2835-audio/
  46 +obj-$(CONFIG_VIDEO_BCM2835)            += bcm2835-camera/
  47 +obj-$(CONFIG_BCM2835_VCHIQ_MMAL)       += vchiq-mmal/
  48 +obj-$(CONFIG_BCM_VC_SM_CMA)            += vc-sm-cma/
  49 +obj-$(CONFIG_VIDEO_CODEC_BCM2835)      += bcm2835-codec/
  50
  51  ccflags-y += -Idrivers/staging/vc04_services -D__VCCOREVER__=0x04000000
  52
  53 --- /dev/null
  54 +++ b/drivers/staging/vc04_services/bcm2835-codec/Kconfig
  55 @@ -0,0 +1,11 @@
  56 +config VIDEO_CODEC_BCM2835
  57 +       tristate "BCM2835 Video codec support"
  58 +       depends on MEDIA_SUPPORT
  59 +       depends on VIDEO_V4L2 && (ARCH_BCM2835 || COMPILE_TEST)
  60 +       select BCM2835_VCHIQ_MMAL
  61 +       select VIDEOBUF2_DMA_CONTIG
  62 +       select V4L2_MEM2MEM_DEV
  63 +       help
  64 +         Say Y here to enable the V4L2 video codecs for
  65 +         Broadcom BCM2835 SoC. This operates over the VCHIQ interface
  66 +         to a service running on VideoCore.
  67 --- /dev/null
  68 +++ b/drivers/staging/vc04_services/bcm2835-codec/Makefile
  69 @@ -0,0 +1,8 @@
  70 +# SPDX-License-Identifier: GPL-2.0
  71 +bcm2835-codec-objs := bcm2835-v4l2-codec.o
  72 +
  73 +obj-$(CONFIG_VIDEO_CODEC_BCM2835) += bcm2835-codec.o
  74 +
  75 +ccflags-y += \
  76 +       -Idrivers/staging/vc04_services \
  77 +       -D__VCCOREVER__=0x04000000
  78 --- /dev/null
  79 +++ b/drivers/staging/vc04_services/bcm2835-codec/TODO
  80 @@ -0,0 +1,24 @@
  81 +1) Convert to be a platform driver.
  82 +
  83 +Right now when the module probes, it tries to initialize VCHI and
  84 +errors out if it wasn't ready yet.  If bcm2835-v4l2 was built in, then
  85 +VCHI generally isn't ready because it depends on both the firmware and
  86 +mailbox drivers having already loaded.
  87 +
  88 +We should have VCHI create a platform device once it's initialized,
  89 +and have this driver bind to it, so that we automatically load the
  90 +v4l2 module after VCHI loads.
  91 +
  92 +2) Support SELECTION API to define crop region on the image for encode.
  93 +
  94 +Particularly for resolutions that aren't a multiple of the macroblock
  95 +size, the codec will report a resolution that is a multiple of the macroblock
  96 +size (it has to have the memory to decode into), and then a different crop
  97 +region within that buffer.
  98 +The most common example is 1080P, where the buffer will be 1920x1088 with a
  99 +crop region of 1920x1080.
 100 +
 101 +3) Refactor so that the component creation is only on queue_setup, not open.
 102 +
 103 +Fixes v4l2-compliance failure on trying to open 100 instances of the
 104 +device.
 105 \ No newline at end of file
 106 --- /dev/null
 107 +++ b/drivers/staging/vc04_services/bcm2835-codec/bcm2835-v4l2-codec.c
 108 @@ -0,0 +1,2359 @@
 109 +// SPDX-License-Identifier: GPL-2.0
 110 +
 111 +/*
 112 + * A v4l2-mem2mem device that wraps the video codec MMAL component.
 113 + *
 114 + * Copyright 2018 Raspberry Pi (Trading) Ltd.
 115 + * Author: Dave Stevenson (dave.stevenson@raspberrypi.org)
 116 + *
 117 + * Loosely based on the vim2m virtual driver by Pawel Osciak
 118 + * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
 119 + * Pawel Osciak, <pawel@osciak.com>
 120 + * Marek Szyprowski, <m.szyprowski@samsung.com>
 121 + *
 122 + * Whilst this driver uses the v4l2_mem2mem framework, it does not need the
 123 + * scheduling aspects, so will always take the buffers, pass them to the VPU,
 124 + * and then signal the job as complete.
 125 + *
 126 + * This program is free software; you can redistribute it and/or modify
 127 + * it under the terms of the GNU General Public License as published by the
 128 + * Free Software Foundation; either version 2 of the
 129 + * License, or (at your option) any later version
 130 + */
 131 +#include <linux/module.h>
 132 +#include <linux/delay.h>
 133 +#include <linux/fs.h>
 134 +#include <linux/timer.h>
 135 +#include <linux/sched.h>
 136 +#include <linux/slab.h>
 137 +#include <linux/platform_device.h>
 138 +#include <linux/syscalls.h>
 139 +
 140 +#include <media/v4l2-mem2mem.h>
 141 +#include <media/v4l2-device.h>
 142 +#include <media/v4l2-ioctl.h>
 143 +#include <media/v4l2-ctrls.h>
 144 +#include <media/v4l2-event.h>
 145 +#include <media/videobuf2-dma-contig.h>
 146 +
 147 +#include "vchiq-mmal/mmal-encodings.h"
 148 +#include "vchiq-mmal/mmal-msg.h"
 149 +#include "vchiq-mmal/mmal-parameters.h"
 150 +#include "vchiq-mmal/mmal-vchiq.h"
 151 +
 152 +/*
 153 + * Default /dev/videoN node numbers for decode and encode.
 154 + * Deliberately avoid the very low numbers as these are often taken by webcams
 155 + * etc, and simple apps tend to only go for /dev/video0.
 156 + */
 157 +static int decode_video_nr = 10;
 158 +module_param(decode_video_nr, int, 0644);
 159 +MODULE_PARM_DESC(decode_video_nr, "decoder video device number");
 160 +
 161 +static int encode_video_nr = 11;
 162 +module_param(encode_video_nr, int, 0644);
 163 +MODULE_PARM_DESC(encode_video_nr, "encoder video device number");
 164 +
 165 +static unsigned int debug;
 166 +module_param(debug, uint, 0644);
 167 +MODULE_PARM_DESC(debug, "activates debug info (0-3)");
 168 +
 169 +#define MIN_W          32
 170 +#define MIN_H          32
 171 +#define MAX_W          1920
 172 +#define MAX_H          1088
 173 +#define BPL_ALIGN      32
 174 +#define DEFAULT_WIDTH  640
 175 +#define DEFAULT_HEIGHT 480
 176 +/*
 177 + * The unanswered question - what is the maximum size of a compressed frame?
 178 + * V4L2 mandates that the encoded frame must fit in a single buffer. Sizing
 179 + * that buffer is a compromise between wasting memory and risking not fitting.
 180 + * The 1080P version of Big Buck Bunny has some frames that exceed 512kB.
 181 + * Adopt a moderately arbitrary split at 720P for switching between 512 and
 182 + * 768kB buffers.
 183 + */
 184 +#define DEF_COMP_BUF_SIZE_GREATER_720P (768 << 10)
 185 +#define DEF_COMP_BUF_SIZE_720P_OR_LESS (512 << 10)
 186 +
 187 +/* Flags that indicate a format can be used for capture/output */
 188 +#define MEM2MEM_CAPTURE                BIT(0)
 189 +#define MEM2MEM_OUTPUT         BIT(1)
 190 +
 191 +#define MEM2MEM_NAME           "bcm2835-codec"
 192 +
 193 +struct bcm2835_codec_fmt {
 194 +       u32     fourcc;
 195 +       int     depth;
 196 +       int     bytesperline_align;
 197 +       u32     flags;
 198 +       u32     mmal_fmt;
 199 +       bool    decode_only;
 200 +       bool    encode_only;
 201 +       int     size_multiplier_x2;
 202 +};
 203 +
 204 +/* Supported raw pixel formats. Those supported for both encode and decode
 205 + * must come first, with those only supported for decode coming after (there
 206 + * are no formats supported for encode only).
 207 + */
 208 +static struct bcm2835_codec_fmt raw_formats[] = {
 209 +       {
 210 +               .fourcc                 = V4L2_PIX_FMT_YUV420,
 211 +               .depth                  = 8,
 212 +               .bytesperline_align     = 32,
 213 +               .flags                  = 0,
 214 +               .mmal_fmt               = MMAL_ENCODING_I420,
 215 +               .size_multiplier_x2     = 3,
 216 +       }, {
 217 +               .fourcc                 = V4L2_PIX_FMT_YVU420,
 218 +               .depth                  = 8,
 219 +               .bytesperline_align     = 32,
 220 +               .flags                  = 0,
 221 +               .mmal_fmt               = MMAL_ENCODING_YV12,
 222 +               .size_multiplier_x2     = 3,
 223 +       }, {
 224 +               .fourcc                 = V4L2_PIX_FMT_NV12,
 225 +               .depth                  = 8,
 226 +               .bytesperline_align     = 32,
 227 +               .flags                  = 0,
 228 +               .mmal_fmt               = MMAL_ENCODING_NV12,
 229 +               .size_multiplier_x2     = 3,
 230 +       }, {
 231 +               .fourcc                 = V4L2_PIX_FMT_NV21,
 232 +               .depth                  = 8,
 233 +               .bytesperline_align     = 32,
 234 +               .flags                  = 0,
 235 +               .mmal_fmt               = MMAL_ENCODING_NV21,
 236 +               .size_multiplier_x2     = 3,
 237 +       }, {
 238 +               .fourcc                 = V4L2_PIX_FMT_RGB565,
 239 +               .depth                  = 16,
 240 +               .bytesperline_align     = 32,
 241 +               .flags                  = 0,
 242 +               .mmal_fmt               = MMAL_ENCODING_RGB16,
 243 +               .size_multiplier_x2     = 2,
 244 +       }, {
 245 +               .fourcc                 = V4L2_PIX_FMT_YUYV,
 246 +               .depth                  = 16,
 247 +               .bytesperline_align     = 32,
 248 +               .flags                  = 0,
 249 +               .mmal_fmt               = MMAL_ENCODING_YUYV,
 250 +               .encode_only            = true,
 251 +               .size_multiplier_x2     = 2,
 252 +       }, {
 253 +               .fourcc                 = V4L2_PIX_FMT_UYVY,
 254 +               .depth                  = 16,
 255 +               .bytesperline_align     = 32,
 256 +               .flags                  = 0,
 257 +               .mmal_fmt               = MMAL_ENCODING_UYVY,
 258 +               .encode_only            = true,
 259 +               .size_multiplier_x2     = 2,
 260 +       }, {
 261 +               .fourcc                 = V4L2_PIX_FMT_YVYU,
 262 +               .depth                  = 16,
 263 +               .bytesperline_align     = 32,
 264 +               .flags                  = 0,
 265 +               .mmal_fmt               = MMAL_ENCODING_YVYU,
 266 +               .encode_only            = true,
 267 +               .size_multiplier_x2     = 2,
 268 +       }, {
 269 +               .fourcc                 = V4L2_PIX_FMT_VYUY,
 270 +               .depth                  = 16,
 271 +               .bytesperline_align     = 32,
 272 +               .flags                  = 0,
 273 +               .mmal_fmt               = MMAL_ENCODING_VYUY,
 274 +               .encode_only            = true,
 275 +               .size_multiplier_x2     = 2,
 276 +       }, {
 277 +               .fourcc                 = V4L2_PIX_FMT_RGB24,
 278 +               .depth                  = 24,
 279 +               .bytesperline_align     = 32,
 280 +               .flags                  = 0,
 281 +               .mmal_fmt               = MMAL_ENCODING_RGB24,
 282 +               .encode_only            = true,
 283 +               .size_multiplier_x2     = 2,
 284 +       }, {
 285 +               .fourcc                 = V4L2_PIX_FMT_BGR24,
 286 +               .depth                  = 24,
 287 +               .bytesperline_align     = 32,
 288 +               .flags                  = 0,
 289 +               .mmal_fmt               = MMAL_ENCODING_BGR24,
 290 +               .encode_only            = true,
 291 +               .size_multiplier_x2     = 2,
 292 +       }, {
 293 +               .fourcc                 = V4L2_PIX_FMT_BGR32,
 294 +               .depth                  = 32,
 295 +               .bytesperline_align     = 32,
 296 +               .flags                  = 0,
 297 +               .mmal_fmt               = MMAL_ENCODING_BGRA,
 298 +               .encode_only            = true,
 299 +               .size_multiplier_x2     = 2,
 300 +       },
 301 +};
 302 +
 303 +/* Supported encoded formats. Those supported for both encode and decode
 304 + * must come first, with those only supported for decode coming after (there
 305 + * are no formats supported for encode only).
 306 + */
 307 +static struct bcm2835_codec_fmt encoded_formats[] = {
 308 +       {
 309 +               .fourcc                 = V4L2_PIX_FMT_H264,
 310 +               .depth                  = 0,
 311 +               .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 312 +               .mmal_fmt               = MMAL_ENCODING_H264,
 313 +       }, {
 314 +               .fourcc                 = V4L2_PIX_FMT_MJPEG,
 315 +               .depth                  = 0,
 316 +               .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 317 +               .mmal_fmt               = MMAL_ENCODING_MJPEG,
 318 +       }, {
 319 +               .fourcc                 = V4L2_PIX_FMT_MPEG4,
 320 +               .depth                  = 0,
 321 +               .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 322 +               .mmal_fmt               = MMAL_ENCODING_MP4V,
 323 +               .decode_only            = true,
 324 +       }, {
 325 +               .fourcc                 = V4L2_PIX_FMT_H263,
 326 +               .depth                  = 0,
 327 +               .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 328 +               .mmal_fmt               = MMAL_ENCODING_H263,
 329 +               .decode_only            = true,
 330 +       }, {
 331 +               .fourcc                 = V4L2_PIX_FMT_MPEG2,
 332 +               .depth                  = 0,
 333 +               .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 334 +               .mmal_fmt               = MMAL_ENCODING_MP2V,
 335 +               .decode_only            = true,
 336 +       }, {
 337 +               .fourcc                 = V4L2_PIX_FMT_VP8,
 338 +               .depth                  = 0,
 339 +               .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 340 +               .mmal_fmt               = MMAL_ENCODING_VP8,
 341 +               .decode_only            = true,
 342 +       },
 343 +       /*
 344 +        * This list couold include VP6 and Theorafor decode, but V4L2 doesn't
 345 +        * support them.
 346 +        */
 347 +};
 348 +
 349 +struct bcm2835_codec_fmt_list {
 350 +       struct bcm2835_codec_fmt *list;
 351 +       unsigned int num_entries;
 352 +};
 353 +
 354 +#define RAW_LIST       0
 355 +#define ENCODED_LIST   1
 356 +
 357 +struct bcm2835_codec_fmt_list formats[] = {
 358 +       {
 359 +               .list = raw_formats,
 360 +               .num_entries = ARRAY_SIZE(raw_formats),
 361 +       }, {
 362 +               .list = encoded_formats,
 363 +               .num_entries = ARRAY_SIZE(encoded_formats),
 364 +       },
 365 +};
 366 +
 367 +struct m2m_mmal_buffer {
 368 +       struct v4l2_m2m_buffer  m2m;
 369 +       struct mmal_buffer      mmal;
 370 +};
 371 +
 372 +/* Per-queue, driver-specific private data */
 373 +struct bcm2835_codec_q_data {
 374 +       /*
 375 +        * These parameters should be treated as gospel, with everything else
 376 +        * being determined from them.
 377 +        */
 378 +       /* Buffer width/height */
 379 +       unsigned int            bytesperline;
 380 +       unsigned int            height;
 381 +       /* Crop size used for selection handling */
 382 +       unsigned int            crop_width;
 383 +       unsigned int            crop_height;
 384 +       bool                    selection_set;
 385 +
 386 +       unsigned int            sizeimage;
 387 +       unsigned int            sequence;
 388 +       struct bcm2835_codec_fmt        *fmt;
 389 +
 390 +       /* One extra buffer header so we can send an EOS. */
 391 +       struct m2m_mmal_buffer  eos_buffer;
 392 +       bool                    eos_buffer_in_use;      /* debug only */
 393 +};
 394 +
 395 +enum {
 396 +       V4L2_M2M_SRC = 0,
 397 +       V4L2_M2M_DST = 1,
 398 +};
 399 +
 400 +static inline struct bcm2835_codec_fmt_list *get_format_list(bool decode,
 401 +                                                            bool capture)
 402 +{
 403 +       return decode ^ capture ? &formats[ENCODED_LIST] : &formats[RAW_LIST];
 404 +}
 405 +
 406 +static struct bcm2835_codec_fmt *get_default_format(bool decode, bool capture)
 407 +{
 408 +       return &get_format_list(decode, capture)->list[0];
 409 +}
 410 +
 411 +static struct bcm2835_codec_fmt *find_format(struct v4l2_format *f, bool decode,
 412 +                                            bool capture)
 413 +{
 414 +       struct bcm2835_codec_fmt *fmt;
 415 +       unsigned int k;
 416 +       struct bcm2835_codec_fmt_list *fmts = get_format_list(decode, capture);
 417 +
 418 +       for (k = 0; k < fmts->num_entries; k++) {
 419 +               fmt = &fmts->list[k];
 420 +               if (fmt->fourcc == f->fmt.pix.pixelformat)
 421 +                       break;
 422 +       }
 423 +
 424 +       /*
 425 +        * Some compressed formats are only supported for decoding, not
 426 +        * encoding.
 427 +        */
 428 +       if (!decode && fmts->list[k].decode_only)
 429 +               return NULL;
 430 +
 431 +       /* Some pixel formats are only supported for encoding, not decoding. */
 432 +       if (decode && fmts->list[k].encode_only)
 433 +               return NULL;
 434 +
 435 +       if (k == fmts->num_entries)
 436 +               return NULL;
 437 +
 438 +       return &fmts->list[k];
 439 +}
 440 +
 441 +struct bcm2835_codec_dev {
 442 +       struct platform_device *pdev;
 443 +
 444 +       /* v4l2 devices */
 445 +       struct v4l2_device      v4l2_dev;
 446 +       struct video_device     vfd;
 447 +       /* mutex for the v4l2 device */
 448 +       struct mutex            dev_mutex;
 449 +       atomic_t                num_inst;
 450 +
 451 +       /* allocated mmal instance and components */
 452 +       bool                    decode;  /* Is this instance a decoder? */
 453 +       struct vchiq_mmal_instance      *instance;
 454 +
 455 +       struct v4l2_m2m_dev     *m2m_dev;
 456 +};
 457 +
 458 +struct bcm2835_codec_ctx {
 459 +       struct v4l2_fh          fh;
 460 +       struct bcm2835_codec_dev        *dev;
 461 +
 462 +       struct v4l2_ctrl_handler hdl;
 463 +
 464 +       struct vchiq_mmal_component  *component;
 465 +       bool component_enabled;
 466 +
 467 +       enum v4l2_colorspace    colorspace;
 468 +       enum v4l2_ycbcr_encoding ycbcr_enc;
 469 +       enum v4l2_xfer_func     xfer_func;
 470 +       enum v4l2_quantization  quant;
 471 +
 472 +       /* Source and destination queue data */
 473 +       struct bcm2835_codec_q_data   q_data[2];
 474 +       s32  bitrate;
 475 +
 476 +       bool aborting;
 477 +       int num_ip_buffers;
 478 +       int num_op_buffers;
 479 +       struct completion frame_cmplt;
 480 +};
 481 +
 482 +struct bcm2835_codec_driver {
 483 +       struct bcm2835_codec_dev *encode;
 484 +       struct bcm2835_codec_dev *decode;
 485 +};
 486 +
 487 +static inline struct bcm2835_codec_ctx *file2ctx(struct file *file)
 488 +{
 489 +       return container_of(file->private_data, struct bcm2835_codec_ctx, fh);
 490 +}
 491 +
 492 +static struct bcm2835_codec_q_data *get_q_data(struct bcm2835_codec_ctx *ctx,
 493 +                                              enum v4l2_buf_type type)
 494 +{
 495 +       switch (type) {
 496 +       case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 497 +               return &ctx->q_data[V4L2_M2M_SRC];
 498 +       case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 499 +               return &ctx->q_data[V4L2_M2M_DST];
 500 +       default:
 501 +               v4l2_err(&ctx->dev->v4l2_dev, "%s: Invalid queue type %u\n",
 502 +                        __func__, type);
 503 +               break;
 504 +       }
 505 +       return NULL;
 506 +}
 507 +
 508 +static struct vchiq_mmal_port *get_port_data(struct bcm2835_codec_ctx *ctx,
 509 +                                            enum v4l2_buf_type type)
 510 +{
 511 +       if (!ctx->component)
 512 +               return NULL;
 513 +
 514 +       switch (type) {
 515 +       case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 516 +               return &ctx->component->input[0];
 517 +       case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 518 +               return &ctx->component->output[0];
 519 +       default:
 520 +               v4l2_err(&ctx->dev->v4l2_dev, "%s: Invalid queue type %u\n",
 521 +                        __func__, type);
 522 +               break;
 523 +       }
 524 +       return NULL;
 525 +}
 526 +
 527 +/*
 528 + * mem2mem callbacks
 529 + */
 530 +
 531 +/**
 532 + * job_ready() - check whether an instance is ready to be scheduled to run
 533 + */
 534 +static int job_ready(void *priv)
 535 +{
 536 +       struct bcm2835_codec_ctx *ctx = priv;
 537 +
 538 +       if (!v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) &&
 539 +           !v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx))
 540 +               return 0;
 541 +
 542 +       return 1;
 543 +}
 544 +
 545 +static void job_abort(void *priv)
 546 +{
 547 +       struct bcm2835_codec_ctx *ctx = priv;
 548 +
 549 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s\n", __func__);
 550 +       /* Will cancel the transaction in the next interrupt handler */
 551 +       ctx->aborting = 1;
 552 +}
 553 +
 554 +static inline unsigned int get_sizeimage(int bpl, int height,
 555 +                                        struct bcm2835_codec_fmt *fmt)
 556 +{
 557 +       return (bpl * height * fmt->size_multiplier_x2) >> 1;
 558 +}
 559 +
 560 +static inline unsigned int get_bytesperline(int width,
 561 +                                           struct bcm2835_codec_fmt *fmt)
 562 +{
 563 +       return ALIGN((width * fmt->depth) >> 3, fmt->bytesperline_align);
 564 +}
 565 +
 566 +static void setup_mmal_port_format(struct bcm2835_codec_ctx *ctx,
 567 +                                  bool decode,
 568 +                                  struct bcm2835_codec_q_data *q_data,
 569 +                                  struct vchiq_mmal_port *port)
 570 +{
 571 +       port->format.encoding = q_data->fmt->mmal_fmt;
 572 +
 573 +       if (!(q_data->fmt->flags & V4L2_FMT_FLAG_COMPRESSED)) {
 574 +               /* Raw image format - set width/height */
 575 +               port->es.video.width = q_data->bytesperline /
 576 +                                               (q_data->fmt->depth >> 3);
 577 +               port->es.video.height = q_data->height;
 578 +               port->es.video.crop.width = q_data->crop_width;
 579 +               port->es.video.crop.height = q_data->crop_height;
 580 +               port->es.video.frame_rate.num = 0;
 581 +               port->es.video.frame_rate.den = 1;
 582 +       } else {
 583 +               /* Compressed format - leave resolution as 0 for decode */
 584 +               if (decode) {
 585 +                       port->es.video.width = 0;
 586 +                       port->es.video.height = 0;
 587 +                       port->es.video.crop.width = 0;
 588 +                       port->es.video.crop.height = 0;
 589 +               } else {
 590 +                       port->es.video.width = q_data->crop_width;
 591 +                       port->es.video.height = q_data->height;
 592 +                       port->es.video.crop.width = q_data->crop_width;
 593 +                       port->es.video.crop.height = q_data->crop_height;
 594 +                       port->format.bitrate = ctx->bitrate;
 595 +               }
 596 +               port->es.video.frame_rate.num = 0;
 597 +               port->es.video.frame_rate.den = 1;
 598 +       }
 599 +       port->es.video.crop.x = 0;
 600 +       port->es.video.crop.y = 0;
 601 +
 602 +       port->current_buffer.size = q_data->sizeimage;
 603 +};
 604 +
 605 +static void ip_buffer_cb(struct vchiq_mmal_instance *instance,
 606 +                        struct vchiq_mmal_port *port, int status,
 607 +                        struct mmal_buffer *mmal_buf)
 608 +{
 609 +       struct bcm2835_codec_ctx *ctx = port->cb_ctx/*, *curr_ctx*/;
 610 +       struct m2m_mmal_buffer *buf =
 611 +                       container_of(mmal_buf, struct m2m_mmal_buffer, mmal);
 612 +
 613 +       v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: port %p buf %p length %lu, flags %x\n",
 614 +                __func__, port, mmal_buf, mmal_buf->length,
 615 +                mmal_buf->mmal_flags);
 616 +
 617 +       if (buf == &ctx->q_data[V4L2_M2M_SRC].eos_buffer) {
 618 +               /* Do we need to add lcoking to prevent multiple submission of
 619 +                * the EOS, and therefore handle mutliple return here?
 620 +                */
 621 +               v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: eos buffer returned.\n",
 622 +                        __func__);
 623 +               ctx->q_data[V4L2_M2M_SRC].eos_buffer_in_use = false;
 624 +               return;
 625 +       }
 626 +
 627 +       if (status) {
 628 +               /* error in transfer */
 629 +               if (buf)
 630 +                       /* there was a buffer with the error so return it */
 631 +                       vb2_buffer_done(&buf->m2m.vb.vb2_buf,
 632 +                                       VB2_BUF_STATE_ERROR);
 633 +               return;
 634 +       }
 635 +       if (mmal_buf->cmd) {
 636 +               v4l2_err(&ctx->dev->v4l2_dev, "%s: Not expecting cmd msgs on ip callback - %08x\n",
 637 +                        __func__, mmal_buf->cmd);
 638 +               /*
 639 +                * CHECKME: Should we return here. The buffer shouldn't have a
 640 +                * message context or vb2 buf associated.
 641 +                */
 642 +       }
 643 +
 644 +       v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: no error. Return buffer %p\n",
 645 +                __func__, &buf->m2m.vb.vb2_buf);
 646 +       vb2_buffer_done(&buf->m2m.vb.vb2_buf, VB2_BUF_STATE_DONE);
 647 +
 648 +       ctx->num_ip_buffers++;
 649 +       v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: done %d input buffers\n",
 650 +                __func__, ctx->num_ip_buffers);
 651 +
 652 +       if (!port->enabled)
 653 +               complete(&ctx->frame_cmplt);
 654 +}
 655 +
 656 +static void queue_res_chg_event(struct bcm2835_codec_ctx *ctx)
 657 +{
 658 +       static const struct v4l2_event ev_src_ch = {
 659 +               .type = V4L2_EVENT_SOURCE_CHANGE,
 660 +               .u.src_change.changes =
 661 +               V4L2_EVENT_SRC_CH_RESOLUTION,
 662 +       };
 663 +
 664 +       v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
 665 +}
 666 +
 667 +static void send_eos_event(struct bcm2835_codec_ctx *ctx)
 668 +{
 669 +       static const struct v4l2_event ev = {
 670 +               .type = V4L2_EVENT_EOS,
 671 +       };
 672 +
 673 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Sending EOS event\n");
 674 +
 675 +       v4l2_event_queue_fh(&ctx->fh, &ev);
 676 +}
 677 +
 678 +static void color_mmal2v4l(struct bcm2835_codec_ctx *ctx, u32 mmal_color_space)
 679 +{
 680 +       switch (mmal_color_space) {
 681 +       case MMAL_COLOR_SPACE_ITUR_BT601:
 682 +               ctx->colorspace = V4L2_COLORSPACE_REC709;
 683 +               ctx->xfer_func = V4L2_XFER_FUNC_709;
 684 +               ctx->ycbcr_enc = V4L2_YCBCR_ENC_601;
 685 +               ctx->quant = V4L2_QUANTIZATION_LIM_RANGE;
 686 +               break;
 687 +
 688 +       case MMAL_COLOR_SPACE_ITUR_BT709:
 689 +               ctx->colorspace = V4L2_COLORSPACE_REC709;
 690 +               ctx->xfer_func = V4L2_XFER_FUNC_709;
 691 +               ctx->ycbcr_enc = V4L2_YCBCR_ENC_709;
 692 +               ctx->quant = V4L2_QUANTIZATION_LIM_RANGE;
 693 +               break;
 694 +       }
 695 +}
 696 +
 697 +static void handle_fmt_changed(struct bcm2835_codec_ctx *ctx,
 698 +                              struct mmal_buffer *mmal_buf)
 699 +{
 700 +       struct bcm2835_codec_q_data *q_data;
 701 +       struct mmal_msg_event_format_changed *format =
 702 +               (struct mmal_msg_event_format_changed *)mmal_buf->buffer;
 703 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format changed: buff size min %u, rec %u, buff num min %u, rec %u\n",
 704 +                __func__,
 705 +                format->buffer_size_min,
 706 +                format->buffer_size_recommended,
 707 +                format->buffer_num_min,
 708 +                format->buffer_num_recommended
 709 +               );
 710 +       if (format->format.type != MMAL_ES_TYPE_VIDEO) {
 711 +               v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format changed but not video %u\n",
 712 +                        __func__, format->format.type);
 713 +               return;
 714 +       }
 715 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format changed to %ux%u, crop %ux%u, colourspace %08X\n",
 716 +                __func__, format->es.video.width, format->es.video.height,
 717 +                format->es.video.crop.width, format->es.video.crop.height,
 718 +                format->es.video.color_space);
 719 +
 720 +       q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
 721 +       q_data->crop_width = format->es.video.crop.width;
 722 +       q_data->crop_height = format->es.video.crop.height;
 723 +       q_data->bytesperline = format->es.video.crop.width;
 724 +       q_data->height = format->es.video.height;
 725 +       q_data->sizeimage = format->buffer_size_min;
 726 +       if (format->es.video.color_space)
 727 +               color_mmal2v4l(ctx, format->es.video.color_space);
 728 +
 729 +       queue_res_chg_event(ctx);
 730 +}
 731 +
 732 +static void op_buffer_cb(struct vchiq_mmal_instance *instance,
 733 +                        struct vchiq_mmal_port *port, int status,
 734 +                        struct mmal_buffer *mmal_buf)
 735 +{
 736 +       struct bcm2835_codec_ctx *ctx = port->cb_ctx;
 737 +       struct m2m_mmal_buffer *buf;
 738 +       struct vb2_v4l2_buffer *vb2;
 739 +
 740 +       v4l2_dbg(2, debug, &ctx->dev->v4l2_dev,
 741 +                "%s: status:%d, buf:%p, length:%lu, flags %u, pts %lld\n",
 742 +                __func__, status, mmal_buf, mmal_buf->length,
 743 +                mmal_buf->mmal_flags, mmal_buf->pts);
 744 +
 745 +       if (status) {
 746 +               /* error in transfer */
 747 +               if (vb2) {
 748 +                       /* there was a buffer with the error so return it */
 749 +                       vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_ERROR);
 750 +               }
 751 +               return;
 752 +       }
 753 +
 754 +       if (mmal_buf->cmd) {
 755 +               switch (mmal_buf->cmd) {
 756 +               case MMAL_EVENT_FORMAT_CHANGED:
 757 +               {
 758 +                       handle_fmt_changed(ctx, mmal_buf);
 759 +                       break;
 760 +               }
 761 +               default:
 762 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Unexpected event on output callback - %08x\n",
 763 +                                __func__, mmal_buf->cmd);
 764 +                       break;
 765 +               }
 766 +               return;
 767 +       }
 768 +
 769 +       buf = container_of(mmal_buf, struct m2m_mmal_buffer, mmal);
 770 +       vb2 = &buf->m2m.vb;
 771 +
 772 +       v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: length %lu, flags %x, idx %u\n",
 773 +                __func__, mmal_buf->length, mmal_buf->mmal_flags,
 774 +                vb2->vb2_buf.index);
 775 +
 776 +       if (mmal_buf->length == 0) {
 777 +               /* stream ended, or buffer being returned during disable. */
 778 +               v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: Empty buffer - flags %04x",
 779 +                        __func__, mmal_buf->mmal_flags);
 780 +               if (!mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS) {
 781 +                       vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_ERROR);
 782 +                       if (!port->enabled)
 783 +                               complete(&ctx->frame_cmplt);
 784 +                       return;
 785 +               }
 786 +       }
 787 +       if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS) {
 788 +               /* EOS packet from the VPU */
 789 +               send_eos_event(ctx);
 790 +               vb2->flags |= V4L2_BUF_FLAG_LAST;
 791 +       }
 792 +
 793 +       vb2->vb2_buf.timestamp = mmal_buf->pts;
 794 +
 795 +       vb2_set_plane_payload(&vb2->vb2_buf, 0, mmal_buf->length);
 796 +       if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
 797 +               vb2->flags |= V4L2_BUF_FLAG_KEYFRAME;
 798 +
 799 +       vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_DONE);
 800 +       ctx->num_op_buffers++;
 801 +
 802 +       v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: done %d output buffers\n",
 803 +                __func__, ctx->num_op_buffers);
 804 +
 805 +       if (!port->enabled)
 806 +               complete(&ctx->frame_cmplt);
 807 +}
 808 +
 809 +/* vb2_to_mmal_buffer() - converts vb2 buffer header to MMAL
 810 + *
 811 + * Copies all the required fields from a VB2 buffer to the MMAL buffer header,
 812 + * ready for sending to the VPU.
 813 + */
 814 +static void vb2_to_mmal_buffer(struct m2m_mmal_buffer *buf,
 815 +                              struct vb2_v4l2_buffer *vb2)
 816 +{
 817 +       buf->mmal.mmal_flags = 0;
 818 +       if (vb2->flags & V4L2_BUF_FLAG_KEYFRAME)
 819 +               buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_FLAG_KEYFRAME;
 820 +
 821 +       /*
 822 +        * Adding this means that the data must be framed correctly as one frame
 823 +        * per buffer. The underlying decoder has no such requirement, but it
 824 +        * will reduce latency as the bistream parser will be kicked immediately
 825 +        * to parse the frame, rather than relying on its own heuristics for
 826 +        * when to wake up.
 827 +        */
 828 +       buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_FLAG_FRAME_END;
 829 +
 830 +       buf->mmal.length = vb2->vb2_buf.planes[0].bytesused;
 831 +       /*
 832 +        * Minor ambiguity in the V4L2 spec as to whether passing in a 0 length
 833 +        * buffer, or one with V4L2_BUF_FLAG_LAST set denotes end of stream.
 834 +        * Handle either.
 835 +        */
 836 +       if (!buf->mmal.length || vb2->flags & V4L2_BUF_FLAG_LAST)
 837 +               buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_FLAG_EOS;
 838 +
 839 +       buf->mmal.pts = vb2->vb2_buf.timestamp;
 840 +       buf->mmal.dts = MMAL_TIME_UNKNOWN;
 841 +}
 842 +
 843 +/* device_run() - prepares and starts the device
 844 + *
 845 + * This simulates all the immediate preparations required before starting
 846 + * a device. This will be called by the framework when it decides to schedule
 847 + * a particular instance.
 848 + */
 849 +static void device_run(void *priv)
 850 +{
 851 +       struct bcm2835_codec_ctx *ctx = priv;
 852 +       struct bcm2835_codec_dev *dev = ctx->dev;
 853 +       struct vb2_v4l2_buffer *src_buf, *dst_buf;
 854 +       struct m2m_mmal_buffer *src_m2m_buf, *dst_m2m_buf;
 855 +       struct v4l2_m2m_buffer *m2m;
 856 +       int ret;
 857 +
 858 +       v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: off we go\n", __func__);
 859 +
 860 +       src_buf = v4l2_m2m_buf_remove(&ctx->fh.m2m_ctx->out_q_ctx);
 861 +       if (src_buf) {
 862 +               m2m = container_of(src_buf, struct v4l2_m2m_buffer, vb);
 863 +               src_m2m_buf = container_of(m2m, struct m2m_mmal_buffer, m2m);
 864 +               vb2_to_mmal_buffer(src_m2m_buf, src_buf);
 865 +
 866 +               ret = vchiq_mmal_submit_buffer(dev->instance,
 867 +                                              &ctx->component->input[0],
 868 +                                              &src_m2m_buf->mmal);
 869 +               v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: Submitted ip buffer len %lu, pts %llu, flags %04x\n",
 870 +                        __func__, src_m2m_buf->mmal.length,
 871 +                        src_m2m_buf->mmal.pts, src_m2m_buf->mmal.mmal_flags);
 872 +               if (ret)
 873 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed submitting ip buffer\n",
 874 +                                __func__);
 875 +       }
 876 +
 877 +       dst_buf = v4l2_m2m_buf_remove(&ctx->fh.m2m_ctx->cap_q_ctx);
 878 +       if (dst_buf) {
 879 +               m2m = container_of(dst_buf, struct v4l2_m2m_buffer, vb);
 880 +               dst_m2m_buf = container_of(m2m, struct m2m_mmal_buffer, m2m);
 881 +               vb2_to_mmal_buffer(dst_m2m_buf, dst_buf);
 882 +
 883 +               ret = vchiq_mmal_submit_buffer(dev->instance,
 884 +                                              &ctx->component->output[0],
 885 +                                              &dst_m2m_buf->mmal);
 886 +               if (ret)
 887 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed submitting op buffer\n",
 888 +                                __func__);
 889 +       }
 890 +
 891 +       v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: Submitted src %p, dst %p\n",
 892 +                __func__, src_m2m_buf, dst_m2m_buf);
 893 +
 894 +       /* Complete the job here. */
 895 +       v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
 896 +}
 897 +
 898 +/*
 899 + * video ioctls
 900 + */
 901 +static int vidioc_querycap(struct file *file, void *priv,
 902 +                          struct v4l2_capability *cap)
 903 +{
 904 +       strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
 905 +       strncpy(cap->card, MEM2MEM_NAME, sizeof(cap->card) - 1);
 906 +       snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
 907 +                MEM2MEM_NAME);
 908 +       cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
 909 +       cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 910 +       return 0;
 911 +}
 912 +
 913 +static int enum_fmt(struct v4l2_fmtdesc *f, bool decode, bool capture)
 914 +{
 915 +       struct bcm2835_codec_fmt *fmt;
 916 +       struct bcm2835_codec_fmt_list *fmts = get_format_list(decode, capture);
 917 +
 918 +       if (f->index < fmts->num_entries) {
 919 +               /* Format found */
 920 +               /* Check format isn't a decode only format when encoding */
 921 +               if (!decode &&
 922 +                   fmts->list[f->index].decode_only)
 923 +                       return -EINVAL;
 924 +               /* Check format isn't a decode only format when encoding */
 925 +               if (decode &&
 926 +                   fmts->list[f->index].encode_only)
 927 +                       return -EINVAL;
 928 +
 929 +               fmt = &fmts->list[f->index];
 930 +               f->pixelformat = fmt->fourcc;
 931 +               f->flags = fmt->flags;
 932 +               return 0;
 933 +       }
 934 +
 935 +       /* Format not found */
 936 +       return -EINVAL;
 937 +}
 938 +
 939 +static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 940 +                                  struct v4l2_fmtdesc *f)
 941 +{
 942 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
 943 +
 944 +       return enum_fmt(f, ctx->dev->decode, true);
 945 +}
 946 +
 947 +static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 948 +                                  struct v4l2_fmtdesc *f)
 949 +{
 950 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
 951 +
 952 +       return enum_fmt(f, ctx->dev->decode, false);
 953 +}
 954 +
 955 +static int vidioc_g_fmt(struct bcm2835_codec_ctx *ctx, struct v4l2_format *f)
 956 +{
 957 +       struct vb2_queue *vq;
 958 +       struct bcm2835_codec_q_data *q_data;
 959 +
 960 +       vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 961 +       if (!vq)
 962 +               return -EINVAL;
 963 +
 964 +       q_data = get_q_data(ctx, f->type);
 965 +
 966 +       f->fmt.pix.width        = q_data->crop_width;
 967 +       f->fmt.pix.height       = q_data->height;
 968 +       f->fmt.pix.field        = V4L2_FIELD_NONE;
 969 +       f->fmt.pix.pixelformat  = q_data->fmt->fourcc;
 970 +       f->fmt.pix.bytesperline = q_data->bytesperline;
 971 +       f->fmt.pix.sizeimage    = q_data->sizeimage;
 972 +       f->fmt.pix.colorspace   = ctx->colorspace;
 973 +       f->fmt.pix.xfer_func    = ctx->xfer_func;
 974 +       f->fmt.pix.ycbcr_enc    = ctx->ycbcr_enc;
 975 +       f->fmt.pix.quantization = ctx->quant;
 976 +
 977 +       return 0;
 978 +}
 979 +
 980 +static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 981 +                               struct v4l2_format *f)
 982 +{
 983 +       return vidioc_g_fmt(file2ctx(file), f);
 984 +}
 985 +
 986 +static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 987 +                               struct v4l2_format *f)
 988 +{
 989 +       return vidioc_g_fmt(file2ctx(file), f);
 990 +}
 991 +
 992 +static int vidioc_try_fmt(struct v4l2_format *f, struct bcm2835_codec_fmt *fmt)
 993 +{
 994 +       /*
 995 +        * The V4L2 specification requires the driver to correct the format
 996 +        * struct if any of the dimensions is unsupported
 997 +        */
 998 +       if (f->fmt.pix.width > MAX_W)
 999 +               f->fmt.pix.width = MAX_W;
1000 +       if (f->fmt.pix.height > MAX_H)
1001 +               f->fmt.pix.height = MAX_H;
1002 +
1003 +       if (!fmt->flags & V4L2_FMT_FLAG_COMPRESSED) {
1004 +               /* Only clip min w/h on capture. Treat 0x0 as unknown. */
1005 +               if (f->fmt.pix.width < MIN_W)
1006 +                       f->fmt.pix.width = MIN_W;
1007 +               if (f->fmt.pix.height < MIN_H)
1008 +                       f->fmt.pix.height = MIN_H;
1009 +
1010 +               /*
1011 +                * Buffer must have a vertical alignment of 16 lines.
1012 +                * The selection will reflect any cropping rectangle when only
1013 +                * some of the pixels are active.
1014 +                */
1015 +               f->fmt.pix.height = ALIGN(f->fmt.pix.height, 16);
1016 +
1017 +               f->fmt.pix.bytesperline = get_bytesperline(f->fmt.pix.width,
1018 +                                                          fmt);
1019 +               f->fmt.pix.sizeimage = get_sizeimage(f->fmt.pix.bytesperline,
1020 +                                                    f->fmt.pix.height,
1021 +                                                    fmt);
1022 +       } else {
1023 +               u32 min_size = f->fmt.pix.width > 1280 ||
1024 +                              f->fmt.pix.height > 720 ?
1025 +                              DEF_COMP_BUF_SIZE_GREATER_720P :
1026 +                              DEF_COMP_BUF_SIZE_720P_OR_LESS;
1027 +
1028 +               f->fmt.pix.bytesperline = 0;
1029 +               if (f->fmt.pix.sizeimage < min_size)
1030 +                       f->fmt.pix.sizeimage = min_size;
1031 +       }
1032 +
1033 +       f->fmt.pix.field = V4L2_FIELD_NONE;
1034 +
1035 +       return 0;
1036 +}
1037 +
1038 +static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
1039 +                                 struct v4l2_format *f)
1040 +{
1041 +       struct bcm2835_codec_fmt *fmt;
1042 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
1043 +
1044 +       fmt = find_format(f, ctx->dev->decode, true);
1045 +       if (!fmt) {
1046 +               f->fmt.pix.pixelformat = get_default_format(ctx->dev->decode,
1047 +                                                           true)->fourcc;
1048 +               fmt = find_format(f, ctx->dev->decode, true);
1049 +       }
1050 +
1051 +       return vidioc_try_fmt(f, fmt);
1052 +}
1053 +
1054 +static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
1055 +                                 struct v4l2_format *f)
1056 +{
1057 +       struct bcm2835_codec_fmt *fmt;
1058 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
1059 +
1060 +       fmt = find_format(f, ctx->dev->decode, false);
1061 +       if (!fmt) {
1062 +               f->fmt.pix.pixelformat = get_default_format(ctx->dev->decode,
1063 +                                                           false)->fourcc;
1064 +               fmt = find_format(f, ctx->dev->decode, false);
1065 +       }
1066 +
1067 +       if (!f->fmt.pix.colorspace)
1068 +               f->fmt.pix.colorspace = ctx->colorspace;
1069 +
1070 +       return vidioc_try_fmt(f, fmt);
1071 +}
1072 +
1073 +static int vidioc_s_fmt(struct bcm2835_codec_ctx *ctx, struct v4l2_format *f,
1074 +                       unsigned int requested_height)
1075 +{
1076 +       struct bcm2835_codec_q_data *q_data;
1077 +       struct vb2_queue *vq;
1078 +       struct vchiq_mmal_port *port;
1079 +       bool update_capture_port = false;
1080 +       int ret;
1081 +
1082 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Setting format for type %d, wxh: %dx%d, fmt: %08x, size %u\n",
1083 +                f->type, f->fmt.pix.width, f->fmt.pix.height,
1084 +                f->fmt.pix.pixelformat, f->fmt.pix.sizeimage);
1085 +
1086 +       vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
1087 +       if (!vq)
1088 +               return -EINVAL;
1089 +
1090 +       q_data = get_q_data(ctx, f->type);
1091 +       if (!q_data)
1092 +               return -EINVAL;
1093 +
1094 +       if (vb2_is_busy(vq)) {
1095 +               v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
1096 +               return -EBUSY;
1097 +       }
1098 +
1099 +       q_data->fmt = find_format(f, ctx->dev->decode,
1100 +                                 f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE);
1101 +       q_data->crop_width = f->fmt.pix.width;
1102 +       q_data->height = f->fmt.pix.height;
1103 +       if (!q_data->selection_set)
1104 +               q_data->crop_height = requested_height;
1105 +
1106 +       /*
1107 +        * Copying the behaviour of vicodec which retains a single set of
1108 +        * colorspace parameters for both input and output.
1109 +        */
1110 +       ctx->colorspace = f->fmt.pix.colorspace;
1111 +       ctx->xfer_func = f->fmt.pix.xfer_func;
1112 +       ctx->ycbcr_enc = f->fmt.pix.ycbcr_enc;
1113 +       ctx->quant = f->fmt.pix.quantization;
1114 +
1115 +       /* All parameters should have been set correctly by try_fmt */
1116 +       q_data->bytesperline = f->fmt.pix.bytesperline;
1117 +       q_data->sizeimage = f->fmt.pix.sizeimage;
1118 +
1119 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Calulated bpl as %u, size %u\n",
1120 +                q_data->bytesperline, q_data->sizeimage);
1121 +
1122 +       if (ctx->dev->decode && q_data->fmt->flags & V4L2_FMT_FLAG_COMPRESSED &&
1123 +           f->fmt.pix.width && f->fmt.pix.height) {
1124 +               /*
1125 +                * On the decoder, if provided with a resolution on the input
1126 +                * side, then replicate that to the output side.
1127 +                * GStreamer appears not to support V4L2_EVENT_SOURCE_CHANGE,
1128 +                * nor set up a resolution on the output side, therefore
1129 +                * we can't decode anything at a resolution other than the
1130 +                * default one.
1131 +                */
1132 +               struct bcm2835_codec_q_data *q_data_dst =
1133 +                                               &ctx->q_data[V4L2_M2M_DST];
1134 +
1135 +               q_data_dst->crop_width = q_data->crop_width;
1136 +               q_data_dst->crop_height = q_data->crop_height;
1137 +               q_data_dst->height = ALIGN(q_data->crop_height, 16);
1138 +
1139 +               q_data_dst->bytesperline =
1140 +                       get_bytesperline(f->fmt.pix.width, q_data_dst->fmt);
1141 +               q_data_dst->sizeimage = get_sizeimage(q_data_dst->bytesperline,
1142 +                                                     q_data_dst->height,
1143 +                                                     q_data_dst->fmt);
1144 +               update_capture_port = true;
1145 +       }
1146 +
1147 +       /* If we have a component then setup the port as well */
1148 +       port = get_port_data(ctx, vq->type);
1149 +       if (!port)
1150 +               return 0;
1151 +
1152 +       setup_mmal_port_format(ctx, ctx->dev->decode, q_data, port);
1153 +       ret = vchiq_mmal_port_set_format(ctx->dev->instance, port);
1154 +       if (ret) {
1155 +               v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed vchiq_mmal_port_set_format on port, ret %d\n",
1156 +                        __func__, ret);
1157 +               ret = -EINVAL;
1158 +       }
1159 +
1160 +       if (q_data->sizeimage < port->minimum_buffer.size) {
1161 +               v4l2_err(&ctx->dev->v4l2_dev, "%s: Current buffer size of %u < min buf size %u - driver mismatch to MMAL\n",
1162 +                        __func__, q_data->sizeimage,
1163 +                        port->minimum_buffer.size);
1164 +       }
1165 +
1166 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Set format for type %d, wxh: %dx%d, fmt: %08x, size %u\n",
1167 +                f->type, q_data->crop_width, q_data->height,
1168 +                q_data->fmt->fourcc, q_data->sizeimage);
1169 +
1170 +       if (update_capture_port) {
1171 +               struct vchiq_mmal_port *port_dst = &ctx->component->output[0];
1172 +               struct bcm2835_codec_q_data *q_data_dst =
1173 +                                               &ctx->q_data[V4L2_M2M_DST];
1174 +
1175 +               setup_mmal_port_format(ctx, ctx->dev->decode, q_data_dst,
1176 +                                      port_dst);
1177 +               ret = vchiq_mmal_port_set_format(ctx->dev->instance, port_dst);
1178 +               if (ret) {
1179 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed vchiq_mmal_port_set_format on output port, ret %d\n",
1180 +                                __func__, ret);
1181 +                       ret = -EINVAL;
1182 +               }
1183 +       }
1184 +       return ret;
1185 +}
1186 +
1187 +static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
1188 +                               struct v4l2_format *f)
1189 +{
1190 +       unsigned int height = f->fmt.pix.height;
1191 +       int ret;
1192 +
1193 +       ret = vidioc_try_fmt_vid_cap(file, priv, f);
1194 +       if (ret)
1195 +               return ret;
1196 +
1197 +       return vidioc_s_fmt(file2ctx(file), f, height);
1198 +}
1199 +
1200 +static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
1201 +                               struct v4l2_format *f)
1202 +{
1203 +       unsigned int height = f->fmt.pix.height;
1204 +       int ret;
1205 +
1206 +       ret = vidioc_try_fmt_vid_out(file, priv, f);
1207 +       if (ret)
1208 +               return ret;
1209 +
1210 +       ret = vidioc_s_fmt(file2ctx(file), f, height);
1211 +       return ret;
1212 +}
1213 +
1214 +static int vidioc_g_selection(struct file *file, void *priv,
1215 +                             struct v4l2_selection *s)
1216 +{
1217 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
1218 +       struct bcm2835_codec_q_data *q_data;
1219 +       bool capture_queue = s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE ?
1220 +                                                               true : false;
1221 +
1222 +       if (capture_queue ^ ctx->dev->decode)
1223 +               /* OUTPUT on decoder and CAPTURE on encoder are not valid. */
1224 +               return -EINVAL;
1225 +
1226 +       q_data = get_q_data(ctx, s->type);
1227 +       if (!q_data)
1228 +               return -EINVAL;
1229 +
1230 +       if (ctx->dev->decode) {
1231 +               switch (s->target) {
1232 +               case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1233 +               case V4L2_SEL_TGT_COMPOSE:
1234 +                       s->r.left = 0;
1235 +                       s->r.top = 0;
1236 +                       s->r.width = q_data->crop_width;
1237 +                       s->r.height = q_data->crop_height;
1238 +                       break;
1239 +               case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1240 +                       s->r.left = 0;
1241 +                       s->r.top = 0;
1242 +                       s->r.width = q_data->crop_width;
1243 +                       s->r.height = q_data->crop_height;
1244 +                       break;
1245 +               default:
1246 +                       return -EINVAL;
1247 +               }
1248 +       } else {
1249 +               switch (s->target) {
1250 +               case V4L2_SEL_TGT_CROP_DEFAULT:
1251 +               case V4L2_SEL_TGT_CROP_BOUNDS:
1252 +                       s->r.top = 0;
1253 +                       s->r.left = 0;
1254 +                       s->r.width = q_data->bytesperline;
1255 +                       s->r.height = q_data->height;
1256 +                       break;
1257 +               case V4L2_SEL_TGT_CROP:
1258 +                       s->r.top = 0;
1259 +                       s->r.left = 0;
1260 +                       s->r.width = q_data->crop_width;
1261 +                       s->r.height = q_data->crop_height;
1262 +                       break;
1263 +               default:
1264 +                       return -EINVAL;
1265 +               }
1266 +       }
1267 +
1268 +       return 0;
1269 +}
1270 +
1271 +static int vidioc_s_selection(struct file *file, void *priv,
1272 +                             struct v4l2_selection *s)
1273 +{
1274 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
1275 +       struct bcm2835_codec_q_data *q_data = NULL;
1276 +       bool capture_queue = s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE ?
1277 +                                                               true : false;
1278 +
1279 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: ctx %p, type %d, q_data %p, target %d, rect x/y %d/%d, w/h %ux%u\n",
1280 +                __func__, ctx, s->type, q_data, s->target, s->r.left, s->r.top,
1281 +                s->r.width, s->r.height);
1282 +
1283 +       if (capture_queue ^ ctx->dev->decode)
1284 +               /* OUTPUT on decoder and CAPTURE on encoder are not valid. */
1285 +               return -EINVAL;
1286 +
1287 +       q_data = get_q_data(ctx, s->type);
1288 +       if (!q_data)
1289 +               return -EINVAL;
1290 +
1291 +       if (ctx->dev->decode) {
1292 +               switch (s->target) {
1293 +               case V4L2_SEL_TGT_COMPOSE:
1294 +                       /* Accept cropped image */
1295 +                       s->r.left = 0;
1296 +                       s->r.top = 0;
1297 +                       s->r.width = min(s->r.width, q_data->crop_width);
1298 +                       s->r.height = min(s->r.height, q_data->height);
1299 +                       q_data->crop_width = s->r.width;
1300 +                       q_data->crop_height = s->r.height;
1301 +                       q_data->selection_set = true;
1302 +                       break;
1303 +               default:
1304 +                       return -EINVAL;
1305 +               }
1306 +       } else {
1307 +               switch (s->target) {
1308 +               case V4L2_SEL_TGT_CROP:
1309 +                       /* Only support crop from (0,0) */
1310 +                       s->r.top = 0;
1311 +                       s->r.left = 0;
1312 +                       s->r.width = min(s->r.width, q_data->crop_width);
1313 +                       s->r.height = min(s->r.height, q_data->crop_height);
1314 +                       q_data->crop_width = s->r.width;
1315 +                       q_data->crop_height = s->r.height;
1316 +                       q_data->selection_set = true;
1317 +                       break;
1318 +               default:
1319 +                       return -EINVAL;
1320 +               }
1321 +       }
1322 +
1323 +       return 0;
1324 +}
1325 +
1326 +static int vidioc_subscribe_evt(struct v4l2_fh *fh,
1327 +                               const struct v4l2_event_subscription *sub)
1328 +{
1329 +       switch (sub->type) {
1330 +       case V4L2_EVENT_EOS:
1331 +               return v4l2_event_subscribe(fh, sub, 2, NULL);
1332 +       case V4L2_EVENT_SOURCE_CHANGE:
1333 +               return v4l2_src_change_event_subscribe(fh, sub);
1334 +       default:
1335 +               return v4l2_ctrl_subscribe_event(fh, sub);
1336 +       }
1337 +}
1338 +
1339 +static int bcm2835_codec_set_level_profile(struct bcm2835_codec_ctx *ctx,
1340 +                                          struct v4l2_ctrl *ctrl)
1341 +{
1342 +       struct mmal_parameter_video_profile param;
1343 +       int param_size = sizeof(param);
1344 +       int ret;
1345 +
1346 +       /*
1347 +        * Level and Profile are set via the same MMAL parameter.
1348 +        * Retrieve the current settings and amend the one that has changed.
1349 +        */
1350 +       ret = vchiq_mmal_port_parameter_get(ctx->dev->instance,
1351 +                                           &ctx->component->output[0],
1352 +                                           MMAL_PARAMETER_PROFILE,
1353 +                                           &param,
1354 +                                           &param_size);
1355 +       if (ret)
1356 +               return ret;
1357 +
1358 +       switch (ctrl->id) {
1359 +       case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
1360 +               switch (ctrl->val) {
1361 +               case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
1362 +                       param.profile = MMAL_VIDEO_PROFILE_H264_BASELINE;
1363 +                       break;
1364 +               case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
1365 +                       param.profile =
1366 +                               MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE;
1367 +                       break;
1368 +               case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
1369 +                       param.profile = MMAL_VIDEO_PROFILE_H264_MAIN;
1370 +                       break;
1371 +               case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
1372 +                       param.profile = MMAL_VIDEO_PROFILE_H264_HIGH;
1373 +                       break;
1374 +               default:
1375 +                       /* Should never get here */
1376 +                       break;
1377 +               }
1378 +               break;
1379 +
1380 +       case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
1381 +               switch (ctrl->val) {
1382 +               case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
1383 +                       param.level = MMAL_VIDEO_LEVEL_H264_1;
1384 +                       break;
1385 +               case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
1386 +                       param.level = MMAL_VIDEO_LEVEL_H264_1b;
1387 +                       break;
1388 +               case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
1389 +                       param.level = MMAL_VIDEO_LEVEL_H264_11;
1390 +                       break;
1391 +               case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
1392 +                       param.level = MMAL_VIDEO_LEVEL_H264_12;
1393 +                       break;
1394 +               case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
1395 +                       param.level = MMAL_VIDEO_LEVEL_H264_13;
1396 +                       break;
1397 +               case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
1398 +                       param.level = MMAL_VIDEO_LEVEL_H264_2;
1399 +                       break;
1400 +               case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
1401 +                       param.level = MMAL_VIDEO_LEVEL_H264_21;
1402 +                       break;
1403 +               case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
1404 +                       param.level = MMAL_VIDEO_LEVEL_H264_22;
1405 +                       break;
1406 +               case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
1407 +                       param.level = MMAL_VIDEO_LEVEL_H264_3;
1408 +                       break;
1409 +               case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
1410 +                       param.level = MMAL_VIDEO_LEVEL_H264_31;
1411 +                       break;
1412 +               case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
1413 +                       param.level = MMAL_VIDEO_LEVEL_H264_32;
1414 +                       break;
1415 +               case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
1416 +                       param.level = MMAL_VIDEO_LEVEL_H264_4;
1417 +                       break;
1418 +               default:
1419 +                       /* Should never get here */
1420 +                       break;
1421 +               }
1422 +       }
1423 +       ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1424 +                                           &ctx->component->output[0],
1425 +                                           MMAL_PARAMETER_PROFILE,
1426 +                                           &param,
1427 +                                           param_size);
1428 +
1429 +       return ret;
1430 +}
1431 +
1432 +static int bcm2835_codec_s_ctrl(struct v4l2_ctrl *ctrl)
1433 +{
1434 +       struct bcm2835_codec_ctx *ctx =
1435 +               container_of(ctrl->handler, struct bcm2835_codec_ctx, hdl);
1436 +       int ret = 0;
1437 +
1438 +       switch (ctrl->id) {
1439 +       case V4L2_CID_MPEG_VIDEO_BITRATE:
1440 +               ctx->bitrate = ctrl->val;
1441 +               if (!ctx->component)
1442 +                       break;
1443 +
1444 +               ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1445 +                                                   &ctx->component->output[0],
1446 +                                                   MMAL_PARAMETER_VIDEO_BIT_RATE,
1447 +                                                   &ctrl->val,
1448 +                                                   sizeof(ctrl->val));
1449 +               break;
1450 +
1451 +       case V4L2_CID_MPEG_VIDEO_BITRATE_MODE: {
1452 +               u32 bitrate_mode;
1453 +
1454 +               if (!ctx->component)
1455 +                       break;
1456 +
1457 +               switch (ctrl->val) {
1458 +               default:
1459 +               case V4L2_MPEG_VIDEO_BITRATE_MODE_VBR:
1460 +                       bitrate_mode = MMAL_VIDEO_RATECONTROL_VARIABLE;
1461 +                       break;
1462 +               case V4L2_MPEG_VIDEO_BITRATE_MODE_CBR:
1463 +                       bitrate_mode = MMAL_VIDEO_RATECONTROL_CONSTANT;
1464 +                       break;
1465 +               }
1466 +
1467 +               ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1468 +                                                   &ctx->component->output[0],
1469 +                                                   MMAL_PARAMETER_RATECONTROL,
1470 +                                                   &bitrate_mode,
1471 +                                                   sizeof(bitrate_mode));
1472 +               break;
1473 +       }
1474 +       case V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER:
1475 +               if (!ctx->component)
1476 +                       break;
1477 +
1478 +               ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1479 +                                                   &ctx->component->output[0],
1480 +                                                   MMAL_PARAMETER_VIDEO_ENCODE_INLINE_HEADER,
1481 +                                                   &ctrl->val,
1482 +                                                   sizeof(ctrl->val));
1483 +               break;
1484 +
1485 +       case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD:
1486 +               if (!ctx->component)
1487 +                       break;
1488 +
1489 +               ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1490 +                                                   &ctx->component->output[0],
1491 +                                                   MMAL_PARAMETER_INTRAPERIOD,
1492 +                                                   &ctrl->val,
1493 +                                                   sizeof(ctrl->val));
1494 +               break;
1495 +
1496 +       case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
1497 +       case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
1498 +               if (!ctx->component)
1499 +                       break;
1500 +
1501 +               ret = bcm2835_codec_set_level_profile(ctx, ctrl);
1502 +               break;
1503 +
1504 +       default:
1505 +               v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
1506 +               return -EINVAL;
1507 +       }
1508 +
1509 +       if (ret)
1510 +               v4l2_err(&ctx->dev->v4l2_dev, "Failed setting ctrl %08x, ret %d\n",
1511 +                        ctrl->id, ret);
1512 +       return ret ? -EINVAL : 0;
1513 +}
1514 +
1515 +static const struct v4l2_ctrl_ops bcm2835_codec_ctrl_ops = {
1516 +       .s_ctrl = bcm2835_codec_s_ctrl,
1517 +};
1518 +
1519 +static int vidioc_try_decoder_cmd(struct file *file, void *priv,
1520 +                                 struct v4l2_decoder_cmd *cmd)
1521 +{
1522 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
1523 +
1524 +       if (!ctx->dev->decode)
1525 +               return -EINVAL;
1526 +
1527 +       switch (cmd->cmd) {
1528 +       case V4L2_DEC_CMD_STOP:
1529 +               if (cmd->flags & V4L2_DEC_CMD_STOP_TO_BLACK) {
1530 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: DEC cmd->flags=%u stop to black not supported",
1531 +                                __func__, cmd->flags);
1532 +                       return -EINVAL;
1533 +               }
1534 +               break;
1535 +       case V4L2_DEC_CMD_START:
1536 +               break;
1537 +       default:
1538 +               return -EINVAL;
1539 +       }
1540 +       return 0;
1541 +}
1542 +
1543 +static int vidioc_decoder_cmd(struct file *file, void *priv,
1544 +                             struct v4l2_decoder_cmd *cmd)
1545 +{
1546 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
1547 +       struct bcm2835_codec_q_data *q_data = &ctx->q_data[V4L2_M2M_SRC];
1548 +       int ret;
1549 +
1550 +       v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s, cmd %u", __func__,
1551 +                cmd->cmd);
1552 +       ret = vidioc_try_decoder_cmd(file, priv, cmd);
1553 +       if (ret)
1554 +               return ret;
1555 +
1556 +       switch (cmd->cmd) {
1557 +       case V4L2_DEC_CMD_STOP:
1558 +               if (q_data->eos_buffer_in_use)
1559 +                       v4l2_err(&ctx->dev->v4l2_dev, "EOS buffers already in use\n");
1560 +               q_data->eos_buffer_in_use = true;
1561 +
1562 +               q_data->eos_buffer.mmal.buffer_size = 0;
1563 +               q_data->eos_buffer.mmal.length = 0;
1564 +               q_data->eos_buffer.mmal.mmal_flags =
1565 +                                               MMAL_BUFFER_HEADER_FLAG_EOS;
1566 +               q_data->eos_buffer.mmal.pts = 0;
1567 +               q_data->eos_buffer.mmal.dts = 0;
1568 +
1569 +               if (!ctx->component)
1570 +                       break;
1571 +
1572 +               ret = vchiq_mmal_submit_buffer(ctx->dev->instance,
1573 +                                              &ctx->component->input[0],
1574 +                                              &q_data->eos_buffer.mmal);
1575 +               if (ret)
1576 +                       v4l2_err(&ctx->dev->v4l2_dev,
1577 +                                "%s: EOS buffer submit failed %d\n",
1578 +                                __func__, ret);
1579 +
1580 +               break;
1581 +
1582 +       case V4L2_DEC_CMD_START:
1583 +               /* Do we need to do anything here? */
1584 +               break;
1585 +
1586 +       default:
1587 +               return -EINVAL;
1588 +       }
1589 +
1590 +       return 0;
1591 +}
1592 +
1593 +static int vidioc_try_encoder_cmd(struct file *file, void *priv,
1594 +                                 struct v4l2_encoder_cmd *cmd)
1595 +{
1596 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
1597 +
1598 +       if (ctx->dev->decode)
1599 +               return -EINVAL;
1600 +
1601 +       switch (cmd->cmd) {
1602 +       case V4L2_ENC_CMD_STOP:
1603 +               break;
1604 +
1605 +       case V4L2_ENC_CMD_START:
1606 +               /* Do we need to do anything here? */
1607 +               break;
1608 +       default:
1609 +               return -EINVAL;
1610 +       }
1611 +       return 0;
1612 +}
1613 +
1614 +static int vidioc_encoder_cmd(struct file *file, void *priv,
1615 +                             struct v4l2_encoder_cmd *cmd)
1616 +{
1617 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
1618 +       struct bcm2835_codec_q_data *q_data = &ctx->q_data[V4L2_M2M_SRC];
1619 +       int ret;
1620 +
1621 +       v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s, cmd %u", __func__,
1622 +                cmd->cmd);
1623 +       ret = vidioc_try_encoder_cmd(file, priv, cmd);
1624 +       if (ret)
1625 +               return ret;
1626 +
1627 +       switch (cmd->cmd) {
1628 +       case V4L2_ENC_CMD_STOP:
1629 +               if (q_data->eos_buffer_in_use)
1630 +                       v4l2_err(&ctx->dev->v4l2_dev, "EOS buffers already in use\n");
1631 +               q_data->eos_buffer_in_use = true;
1632 +
1633 +               q_data->eos_buffer.mmal.buffer_size = 0;
1634 +               q_data->eos_buffer.mmal.length = 0;
1635 +               q_data->eos_buffer.mmal.mmal_flags =
1636 +                                               MMAL_BUFFER_HEADER_FLAG_EOS;
1637 +               q_data->eos_buffer.mmal.pts = 0;
1638 +               q_data->eos_buffer.mmal.dts = 0;
1639 +
1640 +               if (!ctx->component)
1641 +                       break;
1642 +
1643 +               ret = vchiq_mmal_submit_buffer(ctx->dev->instance,
1644 +                                              &ctx->component->input[0],
1645 +                                              &q_data->eos_buffer.mmal);
1646 +               if (ret)
1647 +                       v4l2_err(&ctx->dev->v4l2_dev,
1648 +                                "%s: EOS buffer submit failed %d\n",
1649 +                                __func__, ret);
1650 +
1651 +               break;
1652 +       case V4L2_ENC_CMD_START:
1653 +               /* Do we need to do anything here? */
1654 +               break;
1655 +
1656 +       default:
1657 +               return -EINVAL;
1658 +       }
1659 +
1660 +       return 0;
1661 +}
1662 +
1663 +static const struct v4l2_ioctl_ops bcm2835_codec_ioctl_ops = {
1664 +       .vidioc_querycap        = vidioc_querycap,
1665 +
1666 +       .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
1667 +       .vidioc_g_fmt_vid_cap   = vidioc_g_fmt_vid_cap,
1668 +       .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
1669 +       .vidioc_s_fmt_vid_cap   = vidioc_s_fmt_vid_cap,
1670 +
1671 +       .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
1672 +       .vidioc_g_fmt_vid_out   = vidioc_g_fmt_vid_out,
1673 +       .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
1674 +       .vidioc_s_fmt_vid_out   = vidioc_s_fmt_vid_out,
1675 +
1676 +       .vidioc_reqbufs         = v4l2_m2m_ioctl_reqbufs,
1677 +       .vidioc_querybuf        = v4l2_m2m_ioctl_querybuf,
1678 +       .vidioc_qbuf            = v4l2_m2m_ioctl_qbuf,
1679 +       .vidioc_dqbuf           = v4l2_m2m_ioctl_dqbuf,
1680 +       .vidioc_prepare_buf     = v4l2_m2m_ioctl_prepare_buf,
1681 +       .vidioc_create_bufs     = v4l2_m2m_ioctl_create_bufs,
1682 +       .vidioc_expbuf          = v4l2_m2m_ioctl_expbuf,
1683 +
1684 +       .vidioc_streamon        = v4l2_m2m_ioctl_streamon,
1685 +       .vidioc_streamoff       = v4l2_m2m_ioctl_streamoff,
1686 +
1687 +       .vidioc_g_selection     = vidioc_g_selection,
1688 +       .vidioc_s_selection     = vidioc_s_selection,
1689 +
1690 +       .vidioc_subscribe_event = vidioc_subscribe_evt,
1691 +       .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1692 +
1693 +       .vidioc_decoder_cmd = vidioc_decoder_cmd,
1694 +       .vidioc_try_decoder_cmd = vidioc_try_decoder_cmd,
1695 +       .vidioc_encoder_cmd = vidioc_encoder_cmd,
1696 +       .vidioc_try_encoder_cmd = vidioc_try_encoder_cmd,
1697 +};
1698 +
1699 +static int bcm2835_codec_set_ctrls(struct bcm2835_codec_ctx *ctx)
1700 +{
1701 +       /*
1702 +        * Query the control handler for the value of the various controls and
1703 +        * set them.
1704 +        */
1705 +       const u32 control_ids[] = {
1706 +               V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
1707 +               V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER,
1708 +               V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
1709 +               V4L2_CID_MPEG_VIDEO_H264_LEVEL,
1710 +               V4L2_CID_MPEG_VIDEO_H264_PROFILE,
1711 +       };
1712 +       int i;
1713 +
1714 +       for (i = 0; i < ARRAY_SIZE(control_ids); i++) {
1715 +               struct v4l2_ctrl *ctrl;
1716 +
1717 +               ctrl = v4l2_ctrl_find(&ctx->hdl, control_ids[i]);
1718 +               if (ctrl)
1719 +                       bcm2835_codec_s_ctrl(ctrl);
1720 +       }
1721 +
1722 +       return 0;
1723 +}
1724 +
1725 +static int bcm2835_codec_create_component(struct bcm2835_codec_ctx *ctx)
1726 +{
1727 +       struct bcm2835_codec_dev *dev = ctx->dev;
1728 +       unsigned int enable = 1;
1729 +       int ret;
1730 +
1731 +       ret = vchiq_mmal_component_init(dev->instance, dev->decode ?
1732 +                                       "ril.video_decode" : "ril.video_encode",
1733 +                                       &ctx->component);
1734 +       if (ret < 0) {
1735 +               v4l2_err(&dev->v4l2_dev, "%s: failed to create component for %s\n",
1736 +                        __func__, dev->decode ? "decode" : "encode");
1737 +               return -ENOMEM;
1738 +       }
1739 +
1740 +       vchiq_mmal_port_parameter_set(dev->instance, &ctx->component->input[0],
1741 +                                     MMAL_PARAMETER_ZERO_COPY, &enable,
1742 +                                     sizeof(enable));
1743 +       vchiq_mmal_port_parameter_set(dev->instance, &ctx->component->output[0],
1744 +                                     MMAL_PARAMETER_ZERO_COPY, &enable,
1745 +                                     sizeof(enable));
1746 +
1747 +       setup_mmal_port_format(ctx, dev->decode, &ctx->q_data[V4L2_M2M_SRC],
1748 +                              &ctx->component->input[0]);
1749 +
1750 +       setup_mmal_port_format(ctx, dev->decode, &ctx->q_data[V4L2_M2M_DST],
1751 +                              &ctx->component->output[0]);
1752 +
1753 +       ret = vchiq_mmal_port_set_format(dev->instance,
1754 +                                        &ctx->component->input[0]);
1755 +       if (ret < 0)
1756 +               goto destroy_component;
1757 +
1758 +       ret = vchiq_mmal_port_set_format(dev->instance,
1759 +                                        &ctx->component->output[0]);
1760 +       if (ret < 0)
1761 +               goto destroy_component;
1762 +
1763 +       if (dev->decode) {
1764 +               if (ctx->q_data[V4L2_M2M_DST].sizeimage <
1765 +                       ctx->component->output[0].minimum_buffer.size)
1766 +                       v4l2_err(&dev->v4l2_dev, "buffer size mismatch sizeimage %u < min size %u\n",
1767 +                                ctx->q_data[V4L2_M2M_DST].sizeimage,
1768 +                                ctx->component->output[0].minimum_buffer.size);
1769 +       } else {
1770 +               if (ctx->q_data[V4L2_M2M_SRC].sizeimage <
1771 +                       ctx->component->output[0].minimum_buffer.size)
1772 +                       v4l2_err(&dev->v4l2_dev, "buffer size mismatch sizeimage %u < min size %u\n",
1773 +                                ctx->q_data[V4L2_M2M_SRC].sizeimage,
1774 +                                ctx->component->output[0].minimum_buffer.size);
1775 +
1776 +               /* Now we have a component we can set all the ctrls */
1777 +               bcm2835_codec_set_ctrls(ctx);
1778 +       }
1779 +
1780 +       return 0;
1781 +
1782 +destroy_component:
1783 +       vchiq_mmal_component_finalise(ctx->dev->instance, ctx->component);
1784 +
1785 +       return ret;
1786 +}
1787 +
1788 +/*
1789 + * Queue operations
1790 + */
1791 +
1792 +static int bcm2835_codec_queue_setup(struct vb2_queue *vq,
1793 +                                    unsigned int *nbuffers,
1794 +                                    unsigned int *nplanes,
1795 +                                    unsigned int sizes[],
1796 +                                    struct device *alloc_devs[])
1797 +{
1798 +       struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vq);
1799 +       struct bcm2835_codec_q_data *q_data;
1800 +       struct vchiq_mmal_port *port;
1801 +       unsigned int size;
1802 +
1803 +       q_data = get_q_data(ctx, vq->type);
1804 +       if (!q_data)
1805 +               return -EINVAL;
1806 +
1807 +       if (!ctx->component)
1808 +               if (bcm2835_codec_create_component(ctx))
1809 +                       return -EINVAL;
1810 +
1811 +       port = get_port_data(ctx, vq->type);
1812 +
1813 +       size = q_data->sizeimage;
1814 +
1815 +       if (*nplanes)
1816 +               return sizes[0] < size ? -EINVAL : 0;
1817 +
1818 +       *nplanes = 1;
1819 +
1820 +       sizes[0] = size;
1821 +       port->current_buffer.size = size;
1822 +
1823 +       if (*nbuffers < port->minimum_buffer.num)
1824 +               *nbuffers = port->minimum_buffer.num;
1825 +       /* Add one buffer to take an EOS */
1826 +       port->current_buffer.num = *nbuffers + 1;
1827 +
1828 +       return 0;
1829 +}
1830 +
1831 +static int bcm2835_codec_buf_init(struct vb2_buffer *vb)
1832 +{
1833 +       struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1834 +       struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
1835 +       struct v4l2_m2m_buffer *m2m = container_of(vb2, struct v4l2_m2m_buffer,
1836 +                                                  vb);
1837 +       struct m2m_mmal_buffer *buf = container_of(m2m, struct m2m_mmal_buffer,
1838 +                                                  m2m);
1839 +
1840 +       v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: ctx:%p, vb %p\n",
1841 +                __func__, ctx, vb);
1842 +       buf->mmal.buffer = vb2_plane_vaddr(&buf->m2m.vb.vb2_buf, 0);
1843 +       buf->mmal.buffer_size = vb2_plane_size(&buf->m2m.vb.vb2_buf, 0);
1844 +
1845 +       mmal_vchi_buffer_init(ctx->dev->instance, &buf->mmal);
1846 +
1847 +       return 0;
1848 +}
1849 +
1850 +static int bcm2835_codec_buf_prepare(struct vb2_buffer *vb)
1851 +{
1852 +       struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1853 +       struct bcm2835_codec_q_data *q_data;
1854 +       struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1855 +       struct v4l2_m2m_buffer *m2m = container_of(vbuf, struct v4l2_m2m_buffer,
1856 +                                                  vb);
1857 +       struct m2m_mmal_buffer *buf = container_of(m2m, struct m2m_mmal_buffer,
1858 +                                                  m2m);
1859 +       int ret;
1860 +
1861 +       v4l2_dbg(4, debug, &ctx->dev->v4l2_dev, "%s: type: %d ptr %p\n",
1862 +                __func__, vb->vb2_queue->type, vb);
1863 +
1864 +       q_data = get_q_data(ctx, vb->vb2_queue->type);
1865 +       if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
1866 +               if (vbuf->field == V4L2_FIELD_ANY)
1867 +                       vbuf->field = V4L2_FIELD_NONE;
1868 +               if (vbuf->field != V4L2_FIELD_NONE) {
1869 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s field isn't supported\n",
1870 +                                __func__);
1871 +                       return -EINVAL;
1872 +               }
1873 +       }
1874 +
1875 +       if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
1876 +               v4l2_err(&ctx->dev->v4l2_dev, "%s data will not fit into plane (%lu < %lu)\n",
1877 +                        __func__, vb2_plane_size(vb, 0),
1878 +                        (long)q_data->sizeimage);
1879 +               return -EINVAL;
1880 +       }
1881 +
1882 +       if (!V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type))
1883 +               vb2_set_plane_payload(vb, 0, q_data->sizeimage);
1884 +
1885 +       /*
1886 +        * We want to do this at init, but vb2_core_expbuf checks that the
1887 +        * index < q->num_buffers, and q->num_buffers only gets updated once
1888 +        * all the buffers are allocated.
1889 +        */
1890 +       if (!buf->mmal.dma_buf) {
1891 +               ret = vb2_core_expbuf_dmabuf(vb->vb2_queue,
1892 +                                            vb->vb2_queue->type, vb->index, 0,
1893 +                                            O_CLOEXEC, &buf->mmal.dma_buf);
1894 +               if (ret)
1895 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed to expbuf idx %d, ret %d\n",
1896 +                                __func__, vb->index, ret);
1897 +       } else {
1898 +               ret = 0;
1899 +       }
1900 +
1901 +       return ret;
1902 +}
1903 +
1904 +static void bcm2835_codec_buf_queue(struct vb2_buffer *vb)
1905 +{
1906 +       struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1907 +       struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1908 +
1909 +       v4l2_dbg(4, debug, &ctx->dev->v4l2_dev, "%s: type: %d ptr %p vbuf->flags %u, seq %u, bytesused %u\n",
1910 +                __func__, vb->vb2_queue->type, vb, vbuf->flags, vbuf->sequence,
1911 +                vb->planes[0].bytesused);
1912 +       v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1913 +}
1914 +
1915 +static void bcm2835_codec_buffer_cleanup(struct vb2_buffer *vb)
1916 +{
1917 +       struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1918 +       struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
1919 +       struct v4l2_m2m_buffer *m2m = container_of(vb2, struct v4l2_m2m_buffer,
1920 +                                                  vb);
1921 +       struct m2m_mmal_buffer *buf = container_of(m2m, struct m2m_mmal_buffer,
1922 +                                                  m2m);
1923 +
1924 +       v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: ctx:%p, vb %p\n",
1925 +                __func__, ctx, vb);
1926 +
1927 +       mmal_vchi_buffer_cleanup(&buf->mmal);
1928 +
1929 +       if (buf->mmal.dma_buf) {
1930 +               dma_buf_put(buf->mmal.dma_buf);
1931 +               buf->mmal.dma_buf = NULL;
1932 +       }
1933 +}
1934 +
1935 +static int bcm2835_codec_start_streaming(struct vb2_queue *q,
1936 +                                        unsigned int count)
1937 +{
1938 +       struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(q);
1939 +       struct bcm2835_codec_dev *dev = ctx->dev;
1940 +       struct bcm2835_codec_q_data *q_data = get_q_data(ctx, q->type);
1941 +       int ret;
1942 +
1943 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: type: %d count %d\n",
1944 +                __func__, q->type, count);
1945 +       q_data->sequence = 0;
1946 +
1947 +       if (!ctx->component_enabled) {
1948 +               ret = vchiq_mmal_component_enable(dev->instance,
1949 +                                                 ctx->component);
1950 +               if (ret)
1951 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling component, ret %d\n",
1952 +                                __func__, ret);
1953 +               ctx->component_enabled = true;
1954 +       }
1955 +
1956 +       if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
1957 +               /*
1958 +                * Create the EOS buffer.
1959 +                * We only need the MMAL part, and want to NOT attach a memory
1960 +                * buffer to it as it should only take flags.
1961 +                */
1962 +               memset(&q_data->eos_buffer, 0, sizeof(q_data->eos_buffer));
1963 +               mmal_vchi_buffer_init(dev->instance,
1964 +                                     &q_data->eos_buffer.mmal);
1965 +               q_data->eos_buffer_in_use = false;
1966 +
1967 +               ctx->component->input[0].cb_ctx = ctx;
1968 +               ret = vchiq_mmal_port_enable(dev->instance,
1969 +                                            &ctx->component->input[0],
1970 +                                            ip_buffer_cb);
1971 +               if (ret)
1972 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling i/p port, ret %d\n",
1973 +                                __func__, ret);
1974 +       } else {
1975 +               ctx->component->output[0].cb_ctx = ctx;
1976 +               ret = vchiq_mmal_port_enable(dev->instance,
1977 +                                            &ctx->component->output[0],
1978 +                                            op_buffer_cb);
1979 +               if (ret)
1980 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling o/p port, ret %d\n",
1981 +                                __func__, ret);
1982 +       }
1983 +       return ret;
1984 +}
1985 +
1986 +static void bcm2835_codec_stop_streaming(struct vb2_queue *q)
1987 +{
1988 +       struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(q);
1989 +       struct bcm2835_codec_dev *dev = ctx->dev;
1990 +       struct bcm2835_codec_q_data *q_data = get_q_data(ctx, q->type);
1991 +       struct vchiq_mmal_port *port = get_port_data(ctx, q->type);
1992 +       struct vb2_v4l2_buffer *vbuf;
1993 +       struct vb2_v4l2_buffer *vb2;
1994 +       struct v4l2_m2m_buffer *m2m;
1995 +       struct m2m_mmal_buffer *buf;
1996 +       int ret, i;
1997 +
1998 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: type: %d - return buffers\n",
1999 +                __func__, q->type);
2000 +
2001 +       init_completion(&ctx->frame_cmplt);
2002 +
2003 +       /* Clear out all buffers held by m2m framework */
2004 +       for (;;) {
2005 +               if (V4L2_TYPE_IS_OUTPUT(q->type))
2006 +                       vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
2007 +               else
2008 +                       vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
2009 +               if (!vbuf)
2010 +                       break;
2011 +               v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: return buffer %p\n",
2012 +                        __func__, vbuf);
2013 +
2014 +               v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
2015 +       }
2016 +
2017 +       /* Disable MMAL port - this will flush buffers back */
2018 +       ret = vchiq_mmal_port_disable(dev->instance, port);
2019 +       if (ret)
2020 +               v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed disabling %s port, ret %d\n",
2021 +                        __func__, V4L2_TYPE_IS_OUTPUT(q->type) ? "i/p" : "o/p",
2022 +                        ret);
2023 +
2024 +       while (atomic_read(&port->buffers_with_vpu)) {
2025 +               v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Waiting for buffers to be returned - %d outstanding\n",
2026 +                        __func__, atomic_read(&port->buffers_with_vpu));
2027 +               ret = wait_for_completion_timeout(&ctx->frame_cmplt, HZ);
2028 +               if (ret <= 0) {
2029 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
2030 +                                __func__,
2031 +                                atomic_read(&port->buffers_with_vpu));
2032 +                       break;
2033 +               }
2034 +       }
2035 +
2036 +       /*
2037 +        * Release the VCSM handle here as otherwise REQBUFS(0) aborts because
2038 +        * someone is using the dmabuf before giving the driver a chance to do
2039 +        * anything about it.
2040 +        */
2041 +       for (i = 0; i < q->num_buffers; i++) {
2042 +               vb2 = to_vb2_v4l2_buffer(q->bufs[i]);
2043 +               m2m = container_of(vb2, struct v4l2_m2m_buffer, vb);
2044 +               buf = container_of(m2m, struct m2m_mmal_buffer, m2m);
2045 +
2046 +               mmal_vchi_buffer_cleanup(&buf->mmal);
2047 +               if (buf->mmal.dma_buf) {
2048 +                       dma_buf_put(buf->mmal.dma_buf);
2049 +                       buf->mmal.dma_buf = NULL;
2050 +               }
2051 +       }
2052 +
2053 +       /* If both ports disabled, then disable the component */
2054 +       if (!ctx->component->input[0].enabled &&
2055 +           !ctx->component->output[0].enabled) {
2056 +               ret = vchiq_mmal_component_disable(dev->instance,
2057 +                                                  ctx->component);
2058 +               if (ret)
2059 +                       v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling component, ret %d\n",
2060 +                                __func__, ret);
2061 +       }
2062 +
2063 +       if (V4L2_TYPE_IS_OUTPUT(q->type))
2064 +               mmal_vchi_buffer_cleanup(&q_data->eos_buffer.mmal);
2065 +
2066 +       v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: done\n", __func__);
2067 +}
2068 +
2069 +static const struct vb2_ops bcm2835_codec_qops = {
2070 +       .queue_setup     = bcm2835_codec_queue_setup,
2071 +       .buf_init        = bcm2835_codec_buf_init,
2072 +       .buf_prepare     = bcm2835_codec_buf_prepare,
2073 +       .buf_queue       = bcm2835_codec_buf_queue,
2074 +       .buf_cleanup     = bcm2835_codec_buffer_cleanup,
2075 +       .start_streaming = bcm2835_codec_start_streaming,
2076 +       .stop_streaming  = bcm2835_codec_stop_streaming,
2077 +       .wait_prepare    = vb2_ops_wait_prepare,
2078 +       .wait_finish     = vb2_ops_wait_finish,
2079 +};
2080 +
2081 +static int queue_init(void *priv, struct vb2_queue *src_vq,
2082 +                     struct vb2_queue *dst_vq)
2083 +{
2084 +       struct bcm2835_codec_ctx *ctx = priv;
2085 +       int ret;
2086 +
2087 +       src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
2088 +       src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
2089 +       src_vq->drv_priv = ctx;
2090 +       src_vq->buf_struct_size = sizeof(struct m2m_mmal_buffer);
2091 +       src_vq->ops = &bcm2835_codec_qops;
2092 +       src_vq->mem_ops = &vb2_dma_contig_memops;
2093 +       src_vq->dev = &ctx->dev->pdev->dev;
2094 +       src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
2095 +       src_vq->lock = &ctx->dev->dev_mutex;
2096 +
2097 +       ret = vb2_queue_init(src_vq);
2098 +       if (ret)
2099 +               return ret;
2100 +
2101 +       dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2102 +       dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
2103 +       dst_vq->drv_priv = ctx;
2104 +       dst_vq->buf_struct_size = sizeof(struct m2m_mmal_buffer);
2105 +       dst_vq->ops = &bcm2835_codec_qops;
2106 +       dst_vq->mem_ops = &vb2_dma_contig_memops;
2107 +       dst_vq->dev = &ctx->dev->pdev->dev;
2108 +       dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
2109 +       dst_vq->lock = &ctx->dev->dev_mutex;
2110 +
2111 +       return vb2_queue_init(dst_vq);
2112 +}
2113 +
2114 +/*
2115 + * File operations
2116 + */
2117 +static int bcm2835_codec_open(struct file *file)
2118 +{
2119 +       struct bcm2835_codec_dev *dev = video_drvdata(file);
2120 +       struct bcm2835_codec_ctx *ctx = NULL;
2121 +       struct v4l2_ctrl_handler *hdl;
2122 +       int rc = 0;
2123 +
2124 +       v4l2_dbg(1, debug, &dev->v4l2_dev, "Creating instance for %s\n",
2125 +                dev->decode ? "decode" : "encode");
2126 +       if (mutex_lock_interruptible(&dev->dev_mutex)) {
2127 +               v4l2_err(&dev->v4l2_dev, "Mutex fail\n");
2128 +               return -ERESTARTSYS;
2129 +       }
2130 +       ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
2131 +       if (!ctx) {
2132 +               rc = -ENOMEM;
2133 +               goto open_unlock;
2134 +       }
2135 +
2136 +       ctx->q_data[V4L2_M2M_SRC].fmt = get_default_format(dev->decode, false);
2137 +       ctx->q_data[V4L2_M2M_DST].fmt = get_default_format(dev->decode, true);
2138 +       if (dev->decode) {
2139 +               /*
2140 +                * Input width and height are irrelevant as they will be defined
2141 +                * by the bitstream not the format. Required by V4L2 though.
2142 +                */
2143 +               ctx->q_data[V4L2_M2M_SRC].crop_width = DEFAULT_WIDTH;
2144 +               ctx->q_data[V4L2_M2M_SRC].crop_height = DEFAULT_HEIGHT;
2145 +               ctx->q_data[V4L2_M2M_SRC].height = DEFAULT_HEIGHT;
2146 +               ctx->q_data[V4L2_M2M_SRC].bytesperline = 0;
2147 +               ctx->q_data[V4L2_M2M_SRC].sizeimage =
2148 +                                               DEF_COMP_BUF_SIZE_720P_OR_LESS;
2149 +
2150 +               ctx->q_data[V4L2_M2M_DST].crop_width = DEFAULT_WIDTH;
2151 +               ctx->q_data[V4L2_M2M_DST].crop_height = DEFAULT_HEIGHT;
2152 +               ctx->q_data[V4L2_M2M_DST].height = DEFAULT_HEIGHT;
2153 +               ctx->q_data[V4L2_M2M_DST].bytesperline =
2154 +                               get_bytesperline(DEFAULT_WIDTH,
2155 +                                                ctx->q_data[V4L2_M2M_DST].fmt);
2156 +               ctx->q_data[V4L2_M2M_DST].sizeimage =
2157 +                       get_sizeimage(ctx->q_data[V4L2_M2M_DST].bytesperline,
2158 +                                     ctx->q_data[V4L2_M2M_DST].height,
2159 +                                     ctx->q_data[V4L2_M2M_DST].fmt);
2160 +       } else {
2161 +               ctx->q_data[V4L2_M2M_SRC].crop_width = DEFAULT_WIDTH;
2162 +               ctx->q_data[V4L2_M2M_SRC].crop_height = DEFAULT_HEIGHT;
2163 +               ctx->q_data[V4L2_M2M_SRC].height = DEFAULT_HEIGHT;
2164 +               ctx->q_data[V4L2_M2M_SRC].bytesperline =
2165 +                               get_bytesperline(DEFAULT_WIDTH,
2166 +                                                ctx->q_data[V4L2_M2M_SRC].fmt);
2167 +               ctx->q_data[V4L2_M2M_SRC].sizeimage =
2168 +                       get_sizeimage(ctx->q_data[V4L2_M2M_SRC].bytesperline,
2169 +                                     ctx->q_data[V4L2_M2M_SRC].height,
2170 +                                     ctx->q_data[V4L2_M2M_SRC].fmt);
2171 +
2172 +               ctx->q_data[V4L2_M2M_DST].crop_width = DEFAULT_WIDTH;
2173 +               ctx->q_data[V4L2_M2M_DST].crop_height = DEFAULT_HEIGHT;
2174 +               ctx->q_data[V4L2_M2M_DST].bytesperline = 0;
2175 +               ctx->q_data[V4L2_M2M_DST].height = DEFAULT_HEIGHT;
2176 +               ctx->q_data[V4L2_M2M_DST].sizeimage =
2177 +                                               DEF_COMP_BUF_SIZE_720P_OR_LESS;
2178 +       }
2179 +
2180 +       ctx->colorspace = V4L2_COLORSPACE_REC709;
2181 +       ctx->bitrate = 10 * 1000 * 1000;
2182 +
2183 +       /* Initialise V4L2 contexts */
2184 +       v4l2_fh_init(&ctx->fh, video_devdata(file));
2185 +       file->private_data = &ctx->fh;
2186 +       ctx->dev = dev;
2187 +       hdl = &ctx->hdl;
2188 +       if (!dev->decode) {
2189 +               /* Encode controls */
2190 +               v4l2_ctrl_handler_init(hdl, 6);
2191 +
2192 +               v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
2193 +                                      V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
2194 +                                      V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 0,
2195 +                                      V4L2_MPEG_VIDEO_BITRATE_MODE_VBR);
2196 +               v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
2197 +                                 V4L2_CID_MPEG_VIDEO_BITRATE,
2198 +                                 25 * 1000, 25 * 1000 * 1000,
2199 +                                 25 * 1000, 10 * 1000 * 1000);
2200 +               v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
2201 +                                 V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER,
2202 +                                 0, 1,
2203 +                                 1, 0);
2204 +               v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
2205 +                                 V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
2206 +                                 0, 0x7FFFFFFF,
2207 +                                 1, 60);
2208 +               v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
2209 +                                      V4L2_CID_MPEG_VIDEO_H264_LEVEL,
2210 +                                      V4L2_MPEG_VIDEO_H264_LEVEL_4_2,
2211 +                                      ~(BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_0) |
2212 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1B) |
2213 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_1) |
2214 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_2) |
2215 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_3) |
2216 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_0) |
2217 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_1) |
2218 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_2) |
2219 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_0) |
2220 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_1) |
2221 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_2) |
2222 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_0) |
2223 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_1) |
2224 +                                        BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_2)),
2225 +                                      V4L2_MPEG_VIDEO_H264_LEVEL_4_0);
2226 +               v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
2227 +                                      V4L2_CID_MPEG_VIDEO_H264_PROFILE,
2228 +                                      V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
2229 +                                      ~(BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
2230 +                                        BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
2231 +                                        BIT(V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
2232 +                                        BIT(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)),
2233 +                                       V4L2_MPEG_VIDEO_H264_PROFILE_HIGH);
2234 +               if (hdl->error) {
2235 +                       rc = hdl->error;
2236 +                       goto free_ctrl_handler;
2237 +               }
2238 +               ctx->fh.ctrl_handler = hdl;
2239 +               v4l2_ctrl_handler_setup(hdl);
2240 +       }
2241 +
2242 +       ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
2243 +
2244 +       if (IS_ERR(ctx->fh.m2m_ctx)) {
2245 +               rc = PTR_ERR(ctx->fh.m2m_ctx);
2246 +
2247 +               goto free_ctrl_handler;
2248 +       }
2249 +
2250 +       /* Set both queues as buffered as we have buffering in the VPU. That
2251 +        * means that we will be scheduled whenever either an input or output
2252 +        * buffer is available (otherwise one of each are required).
2253 +        */
2254 +       v4l2_m2m_set_src_buffered(ctx->fh.m2m_ctx, true);
2255 +       v4l2_m2m_set_dst_buffered(ctx->fh.m2m_ctx, true);
2256 +
2257 +       v4l2_fh_add(&ctx->fh);
2258 +       atomic_inc(&dev->num_inst);
2259 +
2260 +       mutex_unlock(&dev->dev_mutex);
2261 +       return 0;
2262 +
2263 +free_ctrl_handler:
2264 +       v4l2_ctrl_handler_free(hdl);
2265 +       kfree(ctx);
2266 +open_unlock:
2267 +       mutex_unlock(&dev->dev_mutex);
2268 +       return rc;
2269 +}
2270 +
2271 +static int bcm2835_codec_release(struct file *file)
2272 +{
2273 +       struct bcm2835_codec_dev *dev = video_drvdata(file);
2274 +       struct bcm2835_codec_ctx *ctx = file2ctx(file);
2275 +
2276 +       v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: Releasing instance %p\n",
2277 +                __func__, ctx);
2278 +
2279 +       v4l2_fh_del(&ctx->fh);
2280 +       v4l2_fh_exit(&ctx->fh);
2281 +       v4l2_ctrl_handler_free(&ctx->hdl);
2282 +       mutex_lock(&dev->dev_mutex);
2283 +       v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
2284 +
2285 +       if (ctx->component)
2286 +               vchiq_mmal_component_finalise(dev->instance, ctx->component);
2287 +
2288 +       mutex_unlock(&dev->dev_mutex);
2289 +       kfree(ctx);
2290 +
2291 +       atomic_dec(&dev->num_inst);
2292 +
2293 +       return 0;
2294 +}
2295 +
2296 +static const struct v4l2_file_operations bcm2835_codec_fops = {
2297 +       .owner          = THIS_MODULE,
2298 +       .open           = bcm2835_codec_open,
2299 +       .release        = bcm2835_codec_release,
2300 +       .poll           = v4l2_m2m_fop_poll,
2301 +       .unlocked_ioctl = video_ioctl2,
2302 +       .mmap           = v4l2_m2m_fop_mmap,
2303 +};
2304 +
2305 +static const struct video_device bcm2835_codec_videodev = {
2306 +       .name           = MEM2MEM_NAME,
2307 +       .vfl_dir        = VFL_DIR_M2M,
2308 +       .fops           = &bcm2835_codec_fops,
2309 +       .ioctl_ops      = &bcm2835_codec_ioctl_ops,
2310 +       .minor          = -1,
2311 +       .release        = video_device_release_empty,
2312 +};
2313 +
2314 +static const struct v4l2_m2m_ops m2m_ops = {
2315 +       .device_run     = device_run,
2316 +       .job_ready      = job_ready,
2317 +       .job_abort      = job_abort,
2318 +};
2319 +
2320 +static int bcm2835_codec_create(struct platform_device *pdev,
2321 +                               struct bcm2835_codec_dev **new_dev,
2322 +                               bool decode)
2323 +{
2324 +       struct bcm2835_codec_dev *dev;
2325 +       struct video_device *vfd;
2326 +       struct vchiq_mmal_instance *instance = NULL;
2327 +       int video_nr;
2328 +       int ret;
2329 +
2330 +       dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
2331 +       if (!dev)
2332 +               return -ENOMEM;
2333 +
2334 +       dev->pdev = pdev;
2335 +
2336 +       dev->decode = decode;
2337 +
2338 +       ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
2339 +       if (ret)
2340 +               return ret;
2341 +
2342 +       atomic_set(&dev->num_inst, 0);
2343 +       mutex_init(&dev->dev_mutex);
2344 +
2345 +       dev->vfd = bcm2835_codec_videodev;
2346 +       vfd = &dev->vfd;
2347 +       vfd->lock = &dev->dev_mutex;
2348 +       vfd->v4l2_dev = &dev->v4l2_dev;
2349 +
2350 +       if (dev->decode) {
2351 +               v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
2352 +               v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
2353 +               video_nr = decode_video_nr;
2354 +       } else {
2355 +               v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
2356 +               v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
2357 +               video_nr = encode_video_nr;
2358 +       }
2359 +
2360 +       ret = video_register_device(vfd, VFL_TYPE_GRABBER, video_nr);
2361 +       if (ret) {
2362 +               v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
2363 +               goto unreg_dev;
2364 +       }
2365 +
2366 +       video_set_drvdata(vfd, dev);
2367 +       snprintf(vfd->name, sizeof(vfd->name), "%s",
2368 +                bcm2835_codec_videodev.name);
2369 +       v4l2_info(&dev->v4l2_dev, "Device registered as /dev/video%d\n",
2370 +                 vfd->num);
2371 +
2372 +       *new_dev = dev;
2373 +
2374 +       dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
2375 +       if (IS_ERR(dev->m2m_dev)) {
2376 +               v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
2377 +               ret = PTR_ERR(dev->m2m_dev);
2378 +               goto err_m2m;
2379 +       }
2380 +
2381 +       ret = vchiq_mmal_init(&instance);
2382 +       if (ret < 0)
2383 +               goto err_m2m;
2384 +       dev->instance = instance;
2385 +
2386 +       v4l2_info(&dev->v4l2_dev, "Loaded V4L2 %s codec\n",
2387 +                 dev->decode ? "decode" : "encode");
2388 +       return 0;
2389 +
2390 +err_m2m:
2391 +       v4l2_m2m_release(dev->m2m_dev);
2392 +       video_unregister_device(&dev->vfd);
2393 +unreg_dev:
2394 +       v4l2_device_unregister(&dev->v4l2_dev);
2395 +
2396 +       return ret;
2397 +}
2398 +
2399 +static int bcm2835_codec_destroy(struct bcm2835_codec_dev *dev)
2400 +{
2401 +       if (!dev)
2402 +               return -ENODEV;
2403 +
2404 +       v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME);
2405 +       v4l2_m2m_release(dev->m2m_dev);
2406 +       video_unregister_device(&dev->vfd);
2407 +       v4l2_device_unregister(&dev->v4l2_dev);
2408 +
2409 +       return 0;
2410 +}
2411 +
2412 +static int bcm2835_codec_probe(struct platform_device *pdev)
2413 +{
2414 +       struct bcm2835_codec_driver *drv;
2415 +       int ret = 0;
2416 +
2417 +       drv = devm_kzalloc(&pdev->dev, sizeof(*drv), GFP_KERNEL);
2418 +       if (!drv)
2419 +               return -ENOMEM;
2420 +
2421 +       ret = bcm2835_codec_create(pdev, &drv->encode, false);
2422 +       if (ret)
2423 +               goto out;
2424 +
2425 +       ret = bcm2835_codec_create(pdev, &drv->decode, true);
2426 +       if (ret)
2427 +               goto out;
2428 +
2429 +       platform_set_drvdata(pdev, drv);
2430 +
2431 +       return 0;
2432 +
2433 +out:
2434 +       if (drv->encode) {
2435 +               bcm2835_codec_destroy(drv->encode);
2436 +               drv->encode = NULL;
2437 +       }
2438 +       return ret;
2439 +}
2440 +
2441 +static int bcm2835_codec_remove(struct platform_device *pdev)
2442 +{
2443 +       struct bcm2835_codec_driver *drv = platform_get_drvdata(pdev);
2444 +
2445 +       bcm2835_codec_destroy(drv->encode);
2446 +
2447 +       bcm2835_codec_destroy(drv->decode);
2448 +
2449 +       return 0;
2450 +}
2451 +
2452 +static struct platform_driver bcm2835_v4l2_codec_driver = {
2453 +       .probe = bcm2835_codec_probe,
2454 +       .remove = bcm2835_codec_remove,
2455 +       .driver = {
2456 +                  .name = "bcm2835-codec",
2457 +                  .owner = THIS_MODULE,
2458 +                  },
2459 +};
2460 +
2461 +module_platform_driver(bcm2835_v4l2_codec_driver);
2462 +
2463 +MODULE_DESCRIPTION("BCM2835 codec V4L2 driver");
2464 +MODULE_AUTHOR("Dave Stevenson, <dave.stevenson@raspberrypi.org>");
2465 +MODULE_LICENSE("GPL");
2466 +MODULE_VERSION("0.0.1");
2467 +MODULE_ALIAS("platform:bcm2835-codec");