drivers/staging/vc04_services/bcm2835-codec/bcm2835-v4l2-codec.c

   1 // SPDX-License-Identifier: GPL-2.0
   2
   3 /*
   4  * A v4l2-mem2mem device that wraps the video codec MMAL component.
   5  *
   6  * Copyright 2018 Raspberry Pi (Trading) Ltd.
   7  * Author: Dave Stevenson (dave.stevenson@raspberrypi.com)
   8  *
   9  * Loosely based on the vim2m virtual driver by Pawel Osciak
  10  * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
  11  * Pawel Osciak, <pawel@osciak.com>
  12  * Marek Szyprowski, <m.szyprowski@samsung.com>
  13  *
  14  * Whilst this driver uses the v4l2_mem2mem framework, it does not need the
  15  * scheduling aspects, so will always take the buffers, pass them to the VPU,
  16  * and then signal the job as complete.
  17  *
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by the
  20  * Free Software Foundation; either version 2 of the
  21  * License, or (at your option) any later version
  22  */
  23 #include <linux/module.h>
  24 #include <linux/delay.h>
  25 #include <linux/fs.h>
  26 #include <linux/timer.h>
  27 #include <linux/sched.h>
  28 #include <linux/slab.h>
  29 #include <linux/platform_device.h>
  30 #include <linux/syscalls.h>
  31
  32 #include <media/v4l2-mem2mem.h>
  33 #include <media/v4l2-device.h>
  34 #include <media/v4l2-ioctl.h>
  35 #include <media/v4l2-ctrls.h>
  36 #include <media/v4l2-event.h>
  37 #include <media/videobuf2-dma-contig.h>
  38
  39 #include "vchiq-mmal/mmal-encodings.h"
  40 #include "vchiq-mmal/mmal-msg.h"
  41 #include "vchiq-mmal/mmal-parameters.h"
  42 #include "vchiq-mmal/mmal-vchiq.h"
  43
  44 /*
  45  * Default /dev/videoN node numbers for decode and encode.
  46  * Deliberately avoid the very low numbers as these are often taken by webcams
  47  * etc, and simple apps tend to only go for /dev/video0.
  48  */
  49 static int decode_video_nr = 10;
  50 module_param(decode_video_nr, int, 0644);
  51 MODULE_PARM_DESC(decode_video_nr, "decoder video device number");
  52
  53 static int encode_video_nr = 11;
  54 module_param(encode_video_nr, int, 0644);
  55 MODULE_PARM_DESC(encode_video_nr, "encoder video device number");
  56
  57 static int isp_video_nr = 12;
  58 module_param(isp_video_nr, int, 0644);
  59 MODULE_PARM_DESC(isp_video_nr, "isp video device number");
  60
  61 static int deinterlace_video_nr = 18;
  62 module_param(deinterlace_video_nr, int, 0644);
  63 MODULE_PARM_DESC(deinterlace_video_nr, "deinterlace video device number");
  64
  65 static int encode_image_nr = 31;
  66 module_param(encode_image_nr, int, 0644);
  67 MODULE_PARM_DESC(encode_image_nr, "encoder image device number");
  68
  69 /*
  70  * Workaround for GStreamer v4l2convert component not considering Bayer formats
  71  * as raw, and therefore not considering a V4L2 device that supports them as
  72  * a suitable candidate.
  73  */
  74 static bool disable_bayer;
  75 module_param(disable_bayer, bool, 0644);
  76 MODULE_PARM_DESC(disable_bayer, "Disable support for Bayer formats");
  77
  78 static unsigned int debug;
  79 module_param(debug, uint, 0644);
  80 MODULE_PARM_DESC(debug, "activates debug info (0-3)");
  81
  82 static bool advanced_deinterlace = true;
  83 module_param(advanced_deinterlace, bool, 0644);
  84 MODULE_PARM_DESC(advanced_deinterlace, "Use advanced deinterlace");
  85
  86 static int field_override;
  87 module_param(field_override, int, 0644);
  88 MODULE_PARM_DESC(field_override, "force TB(8)/BT(9) field");
  89
  90 enum bcm2835_codec_role {
  91         DECODE,
  92         ENCODE,
  93         ISP,
  94         DEINTERLACE,
  95         ENCODE_IMAGE,
  96         NUM_ROLES
  97 };
  98
  99 static const char * const roles[] = {
 100         "decode",
 101         "encode",
 102         "isp",
 103         "image_fx",
 104         "encode_image",
 105 };
 106
 107 static const char * const components[] = {
 108         "ril.video_decode",
 109         "ril.video_encode",
 110         "ril.isp",
 111         "ril.image_fx",
 112         "ril.image_encode",
 113 };
 114
 115 /* Timeout for stop_streaming to allow all buffers to return */
 116 #define COMPLETE_TIMEOUT (2 * HZ)
 117
 118 #define MIN_W           32
 119 #define MIN_H           32
 120 #define MAX_W           1920
 121 #define MAX_H           1920
 122 #define BPL_ALIGN       32
 123 /*
 124  * The decoder spec supports the V4L2_EVENT_SOURCE_CHANGE event, but the docs
 125  * seem to want it to always be generated on startup, which prevents the client
 126  * from configuring the CAPTURE queue based on any parsing it has already done
 127  * which may save time and allow allocation of CAPTURE buffers early. Surely
 128  * SOURCE_CHANGE means something has changed, not just "always notify".
 129  *
 130  * For those clients that don't set the CAPTURE resolution, adopt a default
 131  * resolution that is seriously unlikely to be correct, therefore almost
 132  * guaranteed to get the SOURCE_CHANGE event.
 133  */
 134 #define DEFAULT_WIDTH   32
 135 #define DEFAULT_HEIGHT  32
 136 /*
 137  * The unanswered question - what is the maximum size of a compressed frame?
 138  * V4L2 mandates that the encoded frame must fit in a single buffer. Sizing
 139  * that buffer is a compromise between wasting memory and risking not fitting.
 140  * The 1080P version of Big Buck Bunny has some frames that exceed 512kB.
 141  * Adopt a moderately arbitrary split at 720P for switching between 512 and
 142  * 768kB buffers.
 143  */
 144 #define DEF_COMP_BUF_SIZE_GREATER_720P  (768 << 10)
 145 #define DEF_COMP_BUF_SIZE_720P_OR_LESS  (512 << 10)
 146 /* JPEG image can be very large. For paranoid reasons 4MB is used */
 147 #define DEF_COMP_BUF_SIZE_JPEG (4096 << 10)
 148
 149 /* Flags that indicate a format can be used for capture/output */
 150 #define MEM2MEM_CAPTURE         BIT(0)
 151 #define MEM2MEM_OUTPUT          BIT(1)
 152
 153 #define MEM2MEM_NAME            "bcm2835-codec"
 154
 155 struct bcm2835_codec_fmt {
 156         u32     fourcc;
 157         int     depth;
 158         u8      bytesperline_align[NUM_ROLES];
 159         u32     flags;
 160         u32     mmal_fmt;
 161         int     size_multiplier_x2;
 162         bool    is_bayer;
 163 };
 164
 165 static const struct bcm2835_codec_fmt supported_formats[] = {
 166         {
 167                 /* YUV formats */
 168                 .fourcc                 = V4L2_PIX_FMT_YUV420,
 169                 .depth                  = 8,
 170                 .bytesperline_align     = { 32, 64, 64, 32, 32 },
 171                 .flags                  = 0,
 172                 .mmal_fmt               = MMAL_ENCODING_I420,
 173                 .size_multiplier_x2     = 3,
 174         }, {
 175                 .fourcc                 = V4L2_PIX_FMT_YVU420,
 176                 .depth                  = 8,
 177                 .bytesperline_align     = { 32, 64, 64, 32, 32 },
 178                 .flags                  = 0,
 179                 .mmal_fmt               = MMAL_ENCODING_YV12,
 180                 .size_multiplier_x2     = 3,
 181         }, {
 182                 .fourcc                 = V4L2_PIX_FMT_NV12,
 183                 .depth                  = 8,
 184                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 185                 .flags                  = 0,
 186                 .mmal_fmt               = MMAL_ENCODING_NV12,
 187                 .size_multiplier_x2     = 3,
 188         }, {
 189                 .fourcc                 = V4L2_PIX_FMT_NV21,
 190                 .depth                  = 8,
 191                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 192                 .flags                  = 0,
 193                 .mmal_fmt               = MMAL_ENCODING_NV21,
 194                 .size_multiplier_x2     = 3,
 195         }, {
 196                 .fourcc                 = V4L2_PIX_FMT_RGB565,
 197                 .depth                  = 16,
 198                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 199                 .flags                  = 0,
 200                 .mmal_fmt               = MMAL_ENCODING_RGB16,
 201                 .size_multiplier_x2     = 2,
 202         }, {
 203                 .fourcc                 = V4L2_PIX_FMT_YUYV,
 204                 .depth                  = 16,
 205                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 206                 .flags                  = 0,
 207                 .mmal_fmt               = MMAL_ENCODING_YUYV,
 208                 .size_multiplier_x2     = 2,
 209         }, {
 210                 .fourcc                 = V4L2_PIX_FMT_UYVY,
 211                 .depth                  = 16,
 212                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 213                 .flags                  = 0,
 214                 .mmal_fmt               = MMAL_ENCODING_UYVY,
 215                 .size_multiplier_x2     = 2,
 216         }, {
 217                 .fourcc                 = V4L2_PIX_FMT_YVYU,
 218                 .depth                  = 16,
 219                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 220                 .flags                  = 0,
 221                 .mmal_fmt               = MMAL_ENCODING_YVYU,
 222                 .size_multiplier_x2     = 2,
 223         }, {
 224                 .fourcc                 = V4L2_PIX_FMT_VYUY,
 225                 .depth                  = 16,
 226                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 227                 .flags                  = 0,
 228                 .mmal_fmt               = MMAL_ENCODING_VYUY,
 229                 .size_multiplier_x2     = 2,
 230         }, {
 231                 .fourcc                 = V4L2_PIX_FMT_NV12_COL128,
 232                 .depth                  = 8,
 233                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 234                 .flags                  = 0,
 235                 .mmal_fmt               = MMAL_ENCODING_YUVUV128,
 236                 .size_multiplier_x2     = 3,
 237         }, {
 238                 /* RGB formats */
 239                 .fourcc                 = V4L2_PIX_FMT_RGB24,
 240                 .depth                  = 24,
 241                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 242                 .flags                  = 0,
 243                 .mmal_fmt               = MMAL_ENCODING_RGB24,
 244                 .size_multiplier_x2     = 2,
 245         }, {
 246                 .fourcc                 = V4L2_PIX_FMT_BGR24,
 247                 .depth                  = 24,
 248                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 249                 .flags                  = 0,
 250                 .mmal_fmt               = MMAL_ENCODING_BGR24,
 251                 .size_multiplier_x2     = 2,
 252         }, {
 253                 .fourcc                 = V4L2_PIX_FMT_BGR32,
 254                 .depth                  = 32,
 255                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 256                 .flags                  = 0,
 257                 .mmal_fmt               = MMAL_ENCODING_BGRA,
 258                 .size_multiplier_x2     = 2,
 259         }, {
 260                 .fourcc                 = V4L2_PIX_FMT_RGBA32,
 261                 .depth                  = 32,
 262                 .bytesperline_align     = { 32, 32, 32, 32 },
 263                 .flags                  = 0,
 264                 .mmal_fmt               = MMAL_ENCODING_RGBA,
 265                 .size_multiplier_x2     = 2,
 266         }, {
 267                 /* Bayer formats */
 268                 /* 8 bit */
 269                 .fourcc                 = V4L2_PIX_FMT_SRGGB8,
 270                 .depth                  = 8,
 271                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 272                 .flags                  = 0,
 273                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB8,
 274                 .size_multiplier_x2     = 2,
 275                 .is_bayer               = true,
 276         }, {
 277                 .fourcc                 = V4L2_PIX_FMT_SBGGR8,
 278                 .depth                  = 8,
 279                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 280                 .flags                  = 0,
 281                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR8,
 282                 .size_multiplier_x2     = 2,
 283                 .is_bayer               = true,
 284         }, {
 285                 .fourcc                 = V4L2_PIX_FMT_SGRBG8,
 286                 .depth                  = 8,
 287                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 288                 .flags                  = 0,
 289                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG8,
 290                 .size_multiplier_x2     = 2,
 291                 .is_bayer               = true,
 292         }, {
 293                 .fourcc                 = V4L2_PIX_FMT_SGBRG8,
 294                 .depth                  = 8,
 295                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 296                 .flags                  = 0,
 297                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG8,
 298                 .size_multiplier_x2     = 2,
 299                 .is_bayer               = true,
 300         }, {
 301                 /* 10 bit */
 302                 .fourcc                 = V4L2_PIX_FMT_SRGGB10P,
 303                 .depth                  = 10,
 304                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 305                 .flags                  = 0,
 306                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB10P,
 307                 .size_multiplier_x2     = 2,
 308                 .is_bayer               = true,
 309         }, {
 310                 .fourcc                 = V4L2_PIX_FMT_SBGGR10P,
 311                 .depth                  = 10,
 312                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 313                 .flags                  = 0,
 314                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR10P,
 315                 .size_multiplier_x2     = 2,
 316                 .is_bayer               = true,
 317         }, {
 318                 .fourcc                 = V4L2_PIX_FMT_SGRBG10P,
 319                 .depth                  = 10,
 320                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 321                 .flags                  = 0,
 322                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG10P,
 323                 .size_multiplier_x2     = 2,
 324                 .is_bayer               = true,
 325         }, {
 326                 .fourcc                 = V4L2_PIX_FMT_SGBRG10P,
 327                 .depth                  = 10,
 328                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 329                 .flags                  = 0,
 330                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG10P,
 331                 .size_multiplier_x2     = 2,
 332                 .is_bayer               = true,
 333         }, {
 334                 /* 12 bit */
 335                 .fourcc                 = V4L2_PIX_FMT_SRGGB12P,
 336                 .depth                  = 12,
 337                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 338                 .flags                  = 0,
 339                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB12P,
 340                 .size_multiplier_x2     = 2,
 341                 .is_bayer               = true,
 342         }, {
 343                 .fourcc                 = V4L2_PIX_FMT_SBGGR12P,
 344                 .depth                  = 12,
 345                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 346                 .flags                  = 0,
 347                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR12P,
 348                 .size_multiplier_x2     = 2,
 349                 .is_bayer               = true,
 350         }, {
 351                 .fourcc                 = V4L2_PIX_FMT_SGRBG12P,
 352                 .depth                  = 12,
 353                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 354                 .flags                  = 0,
 355                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG12P,
 356                 .size_multiplier_x2     = 2,
 357                 .is_bayer               = true,
 358         }, {
 359                 .fourcc                 = V4L2_PIX_FMT_SGBRG12P,
 360                 .depth                  = 12,
 361                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 362                 .flags                  = 0,
 363                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG12P,
 364                 .size_multiplier_x2     = 2,
 365                 .is_bayer               = true,
 366         }, {
 367                 /* 14 bit */
 368                 .fourcc                 = V4L2_PIX_FMT_SRGGB14P,
 369                 .depth                  = 14,
 370                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 371                 .flags                  = 0,
 372                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB14P,
 373                 .size_multiplier_x2     = 2,
 374                 .is_bayer               = true,
 375         }, {
 376                 .fourcc                 = V4L2_PIX_FMT_SBGGR14P,
 377                 .depth                  = 14,
 378                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 379                 .flags                  = 0,
 380                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR14P,
 381                 .size_multiplier_x2     = 2,
 382                 .is_bayer               = true,
 383
 384         }, {
 385                 .fourcc                 = V4L2_PIX_FMT_SGRBG14P,
 386                 .depth                  = 14,
 387                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 388                 .flags                  = 0,
 389                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG14P,
 390                 .size_multiplier_x2     = 2,
 391                 .is_bayer               = true,
 392         }, {
 393                 .fourcc                 = V4L2_PIX_FMT_SGBRG14P,
 394                 .depth                  = 14,
 395                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 396                 .flags                  = 0,
 397                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG14P,
 398                 .size_multiplier_x2     = 2,
 399                 .is_bayer               = true,
 400         }, {
 401                 /* 16 bit */
 402                 .fourcc                 = V4L2_PIX_FMT_SRGGB16,
 403                 .depth                  = 16,
 404                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 405                 .flags                  = 0,
 406                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB16,
 407                 .size_multiplier_x2     = 2,
 408                 .is_bayer               = true,
 409         }, {
 410                 .fourcc                 = V4L2_PIX_FMT_SBGGR16,
 411                 .depth                  = 16,
 412                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 413                 .flags                  = 0,
 414                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR16,
 415                 .size_multiplier_x2     = 2,
 416                 .is_bayer               = true,
 417         }, {
 418                 .fourcc                 = V4L2_PIX_FMT_SGRBG16,
 419                 .depth                  = 16,
 420                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 421                 .flags                  = 0,
 422                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG16,
 423                 .size_multiplier_x2     = 2,
 424                 .is_bayer               = true,
 425         }, {
 426                 .fourcc                 = V4L2_PIX_FMT_SGBRG16,
 427                 .depth                  = 16,
 428                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 429                 .flags                  = 0,
 430                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG16,
 431                 .size_multiplier_x2     = 2,
 432                 .is_bayer               = true,
 433         }, {
 434                 /* Bayer formats unpacked to 16bpp */
 435                 /* 10 bit */
 436                 .fourcc                 = V4L2_PIX_FMT_SRGGB10,
 437                 .depth                  = 16,
 438                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 439                 .flags                  = 0,
 440                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB10,
 441                 .size_multiplier_x2     = 2,
 442                 .is_bayer               = true,
 443         }, {
 444                 .fourcc                 = V4L2_PIX_FMT_SBGGR10,
 445                 .depth                  = 16,
 446                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 447                 .flags                  = 0,
 448                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR10,
 449                 .size_multiplier_x2     = 2,
 450                 .is_bayer               = true,
 451         }, {
 452                 .fourcc                 = V4L2_PIX_FMT_SGRBG10,
 453                 .depth                  = 16,
 454                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 455                 .flags                  = 0,
 456                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG10,
 457                 .size_multiplier_x2     = 2,
 458                 .is_bayer               = true,
 459         }, {
 460                 .fourcc                 = V4L2_PIX_FMT_SGBRG10,
 461                 .depth                  = 16,
 462                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 463                 .flags                  = 0,
 464                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG10,
 465                 .size_multiplier_x2     = 2,
 466                 .is_bayer               = true,
 467         }, {
 468                 /* 12 bit */
 469                 .fourcc                 = V4L2_PIX_FMT_SRGGB12,
 470                 .depth                  = 16,
 471                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 472                 .flags                  = 0,
 473                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB12,
 474                 .size_multiplier_x2     = 2,
 475                 .is_bayer               = true,
 476         }, {
 477                 .fourcc                 = V4L2_PIX_FMT_SBGGR12,
 478                 .depth                  = 16,
 479                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 480                 .flags                  = 0,
 481                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR12,
 482                 .size_multiplier_x2     = 2,
 483                 .is_bayer               = true,
 484         }, {
 485                 .fourcc                 = V4L2_PIX_FMT_SGRBG12,
 486                 .depth                  = 16,
 487                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 488                 .flags                  = 0,
 489                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG12,
 490                 .size_multiplier_x2     = 2,
 491                 .is_bayer               = true,
 492         }, {
 493                 .fourcc                 = V4L2_PIX_FMT_SGBRG12,
 494                 .depth                  = 16,
 495                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 496                 .flags                  = 0,
 497                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG12,
 498                 .size_multiplier_x2     = 2,
 499                 .is_bayer               = true,
 500         }, {
 501                 /* 14 bit */
 502                 .fourcc                 = V4L2_PIX_FMT_SRGGB14,
 503                 .depth                  = 16,
 504                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 505                 .flags                  = 0,
 506                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB14,
 507                 .size_multiplier_x2     = 2,
 508                 .is_bayer               = true,
 509         }, {
 510                 .fourcc                 = V4L2_PIX_FMT_SBGGR14,
 511                 .depth                  = 16,
 512                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 513                 .flags                  = 0,
 514                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR14,
 515                 .size_multiplier_x2     = 2,
 516                 .is_bayer               = true,
 517         }, {
 518                 .fourcc                 = V4L2_PIX_FMT_SGRBG14,
 519                 .depth                  = 16,
 520                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 521                 .flags                  = 0,
 522                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG14,
 523                 .size_multiplier_x2     = 2,
 524                 .is_bayer               = true,
 525         }, {
 526                 .fourcc                 = V4L2_PIX_FMT_SGBRG14,
 527                 .depth                  = 16,
 528                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 529                 .flags                  = 0,
 530                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG14,
 531                 .size_multiplier_x2     = 2,
 532                 .is_bayer               = true,
 533         }, {
 534                 /* Monochrome MIPI formats */
 535                 /* 8 bit */
 536                 .fourcc                 = V4L2_PIX_FMT_GREY,
 537                 .depth                  = 8,
 538                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 539                 .flags                  = 0,
 540                 .mmal_fmt               = MMAL_ENCODING_GREY,
 541                 .size_multiplier_x2     = 2,
 542         }, {
 543                 /* 10 bit */
 544                 .fourcc                 = V4L2_PIX_FMT_Y10P,
 545                 .depth                  = 10,
 546                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 547                 .flags                  = 0,
 548                 .mmal_fmt               = MMAL_ENCODING_Y10P,
 549                 .size_multiplier_x2     = 2,
 550         }, {
 551                 /* 12 bit */
 552                 .fourcc                 = V4L2_PIX_FMT_Y12P,
 553                 .depth                  = 12,
 554                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 555                 .flags                  = 0,
 556                 .mmal_fmt               = MMAL_ENCODING_Y12P,
 557                 .size_multiplier_x2     = 2,
 558         }, {
 559                 /* 14 bit */
 560                 .fourcc                 = V4L2_PIX_FMT_Y14P,
 561                 .depth                  = 14,
 562                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 563                 .flags                  = 0,
 564                 .mmal_fmt               = MMAL_ENCODING_Y14P,
 565                 .size_multiplier_x2     = 2,
 566         }, {
 567                 /* 16 bit */
 568                 .fourcc                 = V4L2_PIX_FMT_Y16,
 569                 .depth                  = 16,
 570                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 571                 .flags                  = 0,
 572                 .mmal_fmt               = MMAL_ENCODING_Y16,
 573                 .size_multiplier_x2     = 2,
 574         }, {
 575                 /* 10 bit as 16bpp */
 576                 .fourcc                 = V4L2_PIX_FMT_Y10,
 577                 .depth                  = 16,
 578                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 579                 .flags                  = 0,
 580                 .mmal_fmt               = MMAL_ENCODING_Y10,
 581                 .size_multiplier_x2     = 2,
 582         }, {
 583                 /* 12 bit as 16bpp */
 584                 .fourcc                 = V4L2_PIX_FMT_Y12,
 585                 .depth                  = 16,
 586                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 587                 .flags                  = 0,
 588                 .mmal_fmt               = MMAL_ENCODING_Y12,
 589                 .size_multiplier_x2     = 2,
 590         }, {
 591                 /* 14 bit as 16bpp */
 592                 .fourcc                 = V4L2_PIX_FMT_Y14,
 593                 .depth                  = 16,
 594                 .bytesperline_align     = { 32, 32, 32, 32, 32 },
 595                 .flags                  = 0,
 596                 .mmal_fmt               = MMAL_ENCODING_Y14,
 597                 .size_multiplier_x2     = 2,
 598         }, {
 599                 /* Compressed formats */
 600                 .fourcc                 = V4L2_PIX_FMT_H264,
 601                 .depth                  = 0,
 602                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 603                 .mmal_fmt               = MMAL_ENCODING_H264,
 604         }, {
 605                 .fourcc                 = V4L2_PIX_FMT_JPEG,
 606                 .depth                  = 0,
 607                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 608                 .mmal_fmt               = MMAL_ENCODING_JPEG,
 609         }, {
 610                 .fourcc                 = V4L2_PIX_FMT_MJPEG,
 611                 .depth                  = 0,
 612                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 613                 .mmal_fmt               = MMAL_ENCODING_MJPEG,
 614         }, {
 615                 .fourcc                 = V4L2_PIX_FMT_MPEG4,
 616                 .depth                  = 0,
 617                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 618                 .mmal_fmt               = MMAL_ENCODING_MP4V,
 619         }, {
 620                 .fourcc                 = V4L2_PIX_FMT_H263,
 621                 .depth                  = 0,
 622                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 623                 .mmal_fmt               = MMAL_ENCODING_H263,
 624         }, {
 625                 .fourcc                 = V4L2_PIX_FMT_MPEG2,
 626                 .depth                  = 0,
 627                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 628                 .mmal_fmt               = MMAL_ENCODING_MP2V,
 629         }, {
 630                 .fourcc                 = V4L2_PIX_FMT_VC1_ANNEX_G,
 631                 .depth                  = 0,
 632                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 633                 .mmal_fmt               = MMAL_ENCODING_WVC1,
 634         }
 635 };
 636
 637 struct bcm2835_codec_fmt_list {
 638         struct bcm2835_codec_fmt *list;
 639         unsigned int num_entries;
 640 };
 641
 642 struct m2m_mmal_buffer {
 643         struct v4l2_m2m_buffer  m2m;
 644         struct mmal_buffer      mmal;
 645 };
 646
 647 /* Per-queue, driver-specific private data */
 648 struct bcm2835_codec_q_data {
 649         /*
 650          * These parameters should be treated as gospel, with everything else
 651          * being determined from them.
 652          */
 653         /* Buffer width/height */
 654         unsigned int            bytesperline;
 655         unsigned int            height;
 656         /* Crop size used for selection handling */
 657         unsigned int            crop_width;
 658         unsigned int            crop_height;
 659         bool                    selection_set;
 660         struct v4l2_fract       aspect_ratio;
 661         enum v4l2_field         field;
 662
 663         unsigned int            sizeimage;
 664         unsigned int            sequence;
 665         struct bcm2835_codec_fmt        *fmt;
 666
 667         /* One extra buffer header so we can send an EOS. */
 668         struct m2m_mmal_buffer  eos_buffer;
 669         bool                    eos_buffer_in_use;      /* debug only */
 670 };
 671
 672 struct bcm2835_codec_dev {
 673         struct platform_device *pdev;
 674
 675         /* v4l2 devices */
 676         struct v4l2_device      v4l2_dev;
 677         struct video_device     vfd;
 678         /* mutex for the v4l2 device */
 679         struct mutex            dev_mutex;
 680         atomic_t                num_inst;
 681
 682         /* allocated mmal instance and components */
 683         enum bcm2835_codec_role role;
 684         /* The list of formats supported on input and output queues. */
 685         struct bcm2835_codec_fmt_list   supported_fmts[2];
 686
 687         struct vchiq_mmal_instance      *instance;
 688
 689         struct v4l2_m2m_dev     *m2m_dev;
 690 };
 691
 692 struct bcm2835_codec_ctx {
 693         struct v4l2_fh          fh;
 694         struct bcm2835_codec_dev        *dev;
 695
 696         struct v4l2_ctrl_handler hdl;
 697
 698         struct vchiq_mmal_component  *component;
 699         bool component_enabled;
 700
 701         enum v4l2_colorspace    colorspace;
 702         enum v4l2_ycbcr_encoding ycbcr_enc;
 703         enum v4l2_xfer_func     xfer_func;
 704         enum v4l2_quantization  quant;
 705
 706         int hflip;
 707         int vflip;
 708
 709         /* Source and destination queue data */
 710         struct bcm2835_codec_q_data   q_data[2];
 711         s32  bitrate;
 712         unsigned int    framerate_num;
 713         unsigned int    framerate_denom;
 714
 715         bool aborting;
 716         int num_ip_buffers;
 717         int num_op_buffers;
 718         struct completion frame_cmplt;
 719 };
 720
 721 struct bcm2835_codec_driver {
 722         struct platform_device *pdev;
 723         struct media_device     mdev;
 724
 725         struct bcm2835_codec_dev *encode;
 726         struct bcm2835_codec_dev *decode;
 727         struct bcm2835_codec_dev *isp;
 728         struct bcm2835_codec_dev *deinterlace;
 729         struct bcm2835_codec_dev *encode_image;
 730 };
 731
 732 enum {
 733         V4L2_M2M_SRC = 0,
 734         V4L2_M2M_DST = 1,
 735 };
 736
 737 static const struct bcm2835_codec_fmt *get_fmt(u32 mmal_fmt)
 738 {
 739         unsigned int i;
 740
 741         for (i = 0; i < ARRAY_SIZE(supported_formats); i++) {
 742                 if (supported_formats[i].mmal_fmt == mmal_fmt &&
 743                     (!disable_bayer || !supported_formats[i].is_bayer))
 744                         return &supported_formats[i];
 745         }
 746         return NULL;
 747 }
 748
 749 static inline
 750 struct bcm2835_codec_fmt_list *get_format_list(struct bcm2835_codec_dev *dev,
 751                                                bool capture)
 752 {
 753         return &dev->supported_fmts[capture ? 1 : 0];
 754 }
 755
 756 static
 757 struct bcm2835_codec_fmt *get_default_format(struct bcm2835_codec_dev *dev,
 758                                              bool capture)
 759 {
 760         return &dev->supported_fmts[capture ? 1 : 0].list[0];
 761 }
 762
 763 static
 764 struct bcm2835_codec_fmt *find_format_pix_fmt(u32 pix_fmt,
 765                                               struct bcm2835_codec_dev *dev,
 766                                               bool capture)
 767 {
 768         struct bcm2835_codec_fmt *fmt;
 769         unsigned int k;
 770         struct bcm2835_codec_fmt_list *fmts =
 771                                         &dev->supported_fmts[capture ? 1 : 0];
 772
 773         for (k = 0; k < fmts->num_entries; k++) {
 774                 fmt = &fmts->list[k];
 775                 if (fmt->fourcc == pix_fmt)
 776                         break;
 777         }
 778         if (k == fmts->num_entries)
 779                 return NULL;
 780
 781         return &fmts->list[k];
 782 }
 783
 784 static inline
 785 struct bcm2835_codec_fmt *find_format(struct v4l2_format *f,
 786                                       struct bcm2835_codec_dev *dev,
 787                                       bool capture)
 788 {
 789         return find_format_pix_fmt(f->fmt.pix_mp.pixelformat, dev, capture);
 790 }
 791
 792 static inline struct bcm2835_codec_ctx *file2ctx(struct file *file)
 793 {
 794         return container_of(file->private_data, struct bcm2835_codec_ctx, fh);
 795 }
 796
 797 static struct bcm2835_codec_q_data *get_q_data(struct bcm2835_codec_ctx *ctx,
 798                                                enum v4l2_buf_type type)
 799 {
 800         switch (type) {
 801         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 802                 return &ctx->q_data[V4L2_M2M_SRC];
 803         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 804                 return &ctx->q_data[V4L2_M2M_DST];
 805         default:
 806                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Invalid queue type %u\n",
 807                          __func__, type);
 808                 break;
 809         }
 810         return NULL;
 811 }
 812
 813 static struct vchiq_mmal_port *get_port_data(struct bcm2835_codec_ctx *ctx,
 814                                              enum v4l2_buf_type type)
 815 {
 816         if (!ctx->component)
 817                 return NULL;
 818
 819         switch (type) {
 820         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 821                 return &ctx->component->input[0];
 822         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 823                 return &ctx->component->output[0];
 824         default:
 825                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Invalid queue type %u\n",
 826                          __func__, type);
 827                 break;
 828         }
 829         return NULL;
 830 }
 831
 832 /*
 833  * mem2mem callbacks
 834  */
 835
 836 /*
 837  * job_ready() - check whether an instance is ready to be scheduled to run
 838  */
 839 static int job_ready(void *priv)
 840 {
 841         struct bcm2835_codec_ctx *ctx = priv;
 842
 843         if (!v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) &&
 844             !v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx))
 845                 return 0;
 846
 847         return 1;
 848 }
 849
 850 static void job_abort(void *priv)
 851 {
 852         struct bcm2835_codec_ctx *ctx = priv;
 853
 854         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s\n", __func__);
 855         /* Will cancel the transaction in the next interrupt handler */
 856         ctx->aborting = 1;
 857 }
 858
 859 static inline unsigned int get_sizeimage(int bpl, int width, int height,
 860                                          struct bcm2835_codec_fmt *fmt)
 861 {
 862         if (fmt->flags & V4L2_FMT_FLAG_COMPRESSED) {
 863                 if (fmt->fourcc == V4L2_PIX_FMT_JPEG)
 864                         return DEF_COMP_BUF_SIZE_JPEG;
 865
 866                 if (width * height > 1280 * 720)
 867                         return DEF_COMP_BUF_SIZE_GREATER_720P;
 868                 else
 869                         return DEF_COMP_BUF_SIZE_720P_OR_LESS;
 870         }
 871
 872         if (fmt->fourcc != V4L2_PIX_FMT_NV12_COL128)
 873                 return (bpl * height * fmt->size_multiplier_x2) >> 1;
 874
 875         /*
 876          * V4L2_PIX_FMT_NV12_COL128 is 128 pixel wide columns.
 877          * bytesperline is the column stride in lines, so multiply by
 878          * the number of columns and 128.
 879          */
 880         return (ALIGN(width, 128) * bpl);
 881 }
 882
 883 static inline unsigned int get_bytesperline(int width, int height,
 884                                             struct bcm2835_codec_fmt *fmt,
 885                                             enum bcm2835_codec_role role)
 886 {
 887         if (fmt->fourcc != V4L2_PIX_FMT_NV12_COL128)
 888                 return ALIGN((width * fmt->depth) >> 3,
 889                              fmt->bytesperline_align[role]);
 890
 891         /*
 892          * V4L2_PIX_FMT_NV12_COL128 passes the column stride in lines via
 893          * bytesperline.
 894          * The minimum value for this is sufficient for the base luma and chroma
 895          * with no padding.
 896          */
 897         return (height * 3) >> 1;
 898 }
 899
 900 static void setup_mmal_port_format(struct bcm2835_codec_ctx *ctx,
 901                                    struct bcm2835_codec_q_data *q_data,
 902                                    struct vchiq_mmal_port *port)
 903 {
 904         port->format.encoding = q_data->fmt->mmal_fmt;
 905         port->format.flags = 0;
 906
 907         if (!(q_data->fmt->flags & V4L2_FMT_FLAG_COMPRESSED)) {
 908                 if (q_data->fmt->mmal_fmt != MMAL_ENCODING_YUVUV128) {
 909                         /* Raw image format - set width/height */
 910                         port->es.video.width = (q_data->bytesperline << 3) /
 911                                                         q_data->fmt->depth;
 912                         port->es.video.height = q_data->height;
 913                         port->es.video.crop.width = q_data->crop_width;
 914                         port->es.video.crop.height = q_data->crop_height;
 915                 } else {
 916                         /* NV12_COL128 / YUVUV128 column format */
 917                         /* Column stride in lines */
 918                         port->es.video.width = q_data->bytesperline;
 919                         port->es.video.height = q_data->height;
 920                         port->es.video.crop.width = q_data->crop_width;
 921                         port->es.video.crop.height = q_data->crop_height;
 922                         port->format.flags = MMAL_ES_FORMAT_FLAG_COL_FMTS_WIDTH_IS_COL_STRIDE;
 923                 }
 924                 port->es.video.frame_rate.num = ctx->framerate_num;
 925                 port->es.video.frame_rate.den = ctx->framerate_denom;
 926         } else {
 927                 /* Compressed format - leave resolution as 0 for decode */
 928                 if (ctx->dev->role == DECODE) {
 929                         port->es.video.width = 0;
 930                         port->es.video.height = 0;
 931                         port->es.video.crop.width = 0;
 932                         port->es.video.crop.height = 0;
 933                 } else {
 934                         port->es.video.width = q_data->crop_width;
 935                         port->es.video.height = q_data->height;
 936                         port->es.video.crop.width = q_data->crop_width;
 937                         port->es.video.crop.height = q_data->crop_height;
 938                         port->format.bitrate = ctx->bitrate;
 939                         port->es.video.frame_rate.num = ctx->framerate_num;
 940                         port->es.video.frame_rate.den = ctx->framerate_denom;
 941                 }
 942         }
 943         port->es.video.crop.x = 0;
 944         port->es.video.crop.y = 0;
 945
 946         port->current_buffer.size = q_data->sizeimage;
 947 };
 948
 949 static void ip_buffer_cb(struct vchiq_mmal_instance *instance,
 950                          struct vchiq_mmal_port *port, int status,
 951                          struct mmal_buffer *mmal_buf)
 952 {
 953         struct bcm2835_codec_ctx *ctx = port->cb_ctx/*, *curr_ctx*/;
 954         struct m2m_mmal_buffer *buf =
 955                         container_of(mmal_buf, struct m2m_mmal_buffer, mmal);
 956
 957         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: port %p buf %p length %lu, flags %x\n",
 958                  __func__, port, mmal_buf, mmal_buf->length,
 959                  mmal_buf->mmal_flags);
 960
 961         if (buf == &ctx->q_data[V4L2_M2M_SRC].eos_buffer) {
 962                 /* Do we need to add lcoking to prevent multiple submission of
 963                  * the EOS, and therefore handle mutliple return here?
 964                  */
 965                 v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: eos buffer returned.\n",
 966                          __func__);
 967                 ctx->q_data[V4L2_M2M_SRC].eos_buffer_in_use = false;
 968                 return;
 969         }
 970
 971         if (status) {
 972                 /* error in transfer */
 973                 if (buf)
 974                         /* there was a buffer with the error so return it */
 975                         vb2_buffer_done(&buf->m2m.vb.vb2_buf,
 976                                         VB2_BUF_STATE_ERROR);
 977                 return;
 978         }
 979         if (mmal_buf->cmd) {
 980                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Not expecting cmd msgs on ip callback - %08x\n",
 981                          __func__, mmal_buf->cmd);
 982                 /*
 983                  * CHECKME: Should we return here. The buffer shouldn't have a
 984                  * message context or vb2 buf associated.
 985                  */
 986         }
 987
 988         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: no error. Return buffer %p\n",
 989                  __func__, &buf->m2m.vb.vb2_buf);
 990         vb2_buffer_done(&buf->m2m.vb.vb2_buf,
 991                         port->enabled ? VB2_BUF_STATE_DONE :
 992                                         VB2_BUF_STATE_QUEUED);
 993
 994         ctx->num_ip_buffers++;
 995         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: done %d input buffers\n",
 996                  __func__, ctx->num_ip_buffers);
 997
 998         if (!port->enabled && atomic_read(&port->buffers_with_vpu))
 999                 complete(&ctx->frame_cmplt);
1000 }
1001
1002 static void queue_res_chg_event(struct bcm2835_codec_ctx *ctx)
1003 {
1004         static const struct v4l2_event ev_src_ch = {
1005                 .type = V4L2_EVENT_SOURCE_CHANGE,
1006                 .u.src_change.changes =
1007                 V4L2_EVENT_SRC_CH_RESOLUTION,
1008         };
1009
1010         v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
1011 }
1012
1013 static void send_eos_event(struct bcm2835_codec_ctx *ctx)
1014 {
1015         static const struct v4l2_event ev = {
1016                 .type = V4L2_EVENT_EOS,
1017         };
1018
1019         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Sending EOS event\n");
1020
1021         v4l2_event_queue_fh(&ctx->fh, &ev);
1022 }
1023
1024 static void color_mmal2v4l(struct bcm2835_codec_ctx *ctx, u32 encoding,
1025                            u32 color_space)
1026 {
1027         int is_rgb;
1028
1029         switch (encoding) {
1030         case MMAL_ENCODING_I420:
1031         case MMAL_ENCODING_YV12:
1032         case MMAL_ENCODING_NV12:
1033         case MMAL_ENCODING_NV21:
1034         case V4L2_PIX_FMT_YUYV:
1035         case V4L2_PIX_FMT_YVYU:
1036         case V4L2_PIX_FMT_UYVY:
1037         case V4L2_PIX_FMT_VYUY:
1038                 /* YUV based colourspaces */
1039                 switch (color_space) {
1040                 case MMAL_COLOR_SPACE_ITUR_BT601:
1041                         ctx->colorspace = V4L2_COLORSPACE_SMPTE170M;
1042                         break;
1043
1044                 case MMAL_COLOR_SPACE_ITUR_BT709:
1045                         ctx->colorspace = V4L2_COLORSPACE_REC709;
1046                         break;
1047                 default:
1048                         break;
1049                 }
1050                 break;
1051         default:
1052                 /* RGB based colourspaces */
1053                 ctx->colorspace = V4L2_COLORSPACE_SRGB;
1054                 break;
1055         }
1056         ctx->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(ctx->colorspace);
1057         ctx->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(ctx->colorspace);
1058         is_rgb = ctx->colorspace == V4L2_COLORSPACE_SRGB;
1059         ctx->quant = V4L2_MAP_QUANTIZATION_DEFAULT(is_rgb, ctx->colorspace,
1060                                                    ctx->ycbcr_enc);
1061 }
1062
1063 static void handle_fmt_changed(struct bcm2835_codec_ctx *ctx,
1064                                struct mmal_buffer *mmal_buf)
1065 {
1066         struct bcm2835_codec_q_data *q_data;
1067         struct mmal_msg_event_format_changed *format =
1068                 (struct mmal_msg_event_format_changed *)mmal_buf->buffer;
1069         struct mmal_parameter_video_interlace_type interlace;
1070         int interlace_size = sizeof(interlace);
1071         struct vb2_queue *vq;
1072         int ret;
1073
1074         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format changed: buff size min %u, rec %u, buff num min %u, rec %u\n",
1075                  __func__,
1076                  format->buffer_size_min,
1077                  format->buffer_size_recommended,
1078                  format->buffer_num_min,
1079                  format->buffer_num_recommended
1080                 );
1081         if (format->format.type != MMAL_ES_TYPE_VIDEO) {
1082                 v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format changed but not video %u\n",
1083                          __func__, format->format.type);
1084                 return;
1085         }
1086         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format changed to %ux%u, crop %ux%u, colourspace %08X\n",
1087                  __func__, format->es.video.width, format->es.video.height,
1088                  format->es.video.crop.width, format->es.video.crop.height,
1089                  format->es.video.color_space);
1090
1091         q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
1092         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format was %ux%u, crop %ux%u\n",
1093                  __func__, q_data->bytesperline, q_data->height,
1094                  q_data->crop_width, q_data->crop_height);
1095
1096         q_data->crop_width = format->es.video.crop.width;
1097         q_data->crop_height = format->es.video.crop.height;
1098         /*
1099          * Stop S_FMT updating crop_height should it be unaligned.
1100          * Client can still update the crop region via S_SELECTION should it
1101          * really want to, but the decoder is likely to complain that the
1102          * format then doesn't match.
1103          */
1104         q_data->selection_set = true;
1105         q_data->bytesperline = get_bytesperline(format->es.video.width,
1106                                                 format->es.video.height,
1107                                                 q_data->fmt, ctx->dev->role);
1108
1109         q_data->height = format->es.video.height;
1110         q_data->sizeimage = format->buffer_size_min;
1111         if (format->es.video.color_space)
1112                 color_mmal2v4l(ctx, format->format.encoding,
1113                                format->es.video.color_space);
1114
1115         q_data->aspect_ratio.numerator = format->es.video.par.num;
1116         q_data->aspect_ratio.denominator = format->es.video.par.den;
1117
1118         ret = vchiq_mmal_port_parameter_get(ctx->dev->instance,
1119                                             &ctx->component->output[0],
1120                                             MMAL_PARAMETER_VIDEO_INTERLACE_TYPE,
1121                                             &interlace,
1122                                             &interlace_size);
1123         if (!ret) {
1124                 switch (interlace.mode) {
1125                 case MMAL_INTERLACE_PROGRESSIVE:
1126                 default:
1127                         q_data->field = V4L2_FIELD_NONE;
1128                         break;
1129                 case MMAL_INTERLACE_FIELDS_INTERLEAVED_UPPER_FIRST:
1130                         q_data->field = V4L2_FIELD_INTERLACED_TB;
1131                         break;
1132                 case MMAL_INTERLACE_FIELDS_INTERLEAVED_LOWER_FIRST:
1133                         q_data->field = V4L2_FIELD_INTERLACED_BT;
1134                         break;
1135                 }
1136                 v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: interlace mode %u, v4l2 field %u\n",
1137                          __func__, interlace.mode, q_data->field);
1138         } else {
1139                 q_data->field = V4L2_FIELD_NONE;
1140         }
1141
1142         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
1143         if (vq->streaming)
1144                 vq->last_buffer_dequeued = true;
1145
1146         queue_res_chg_event(ctx);
1147 }
1148
1149 static void op_buffer_cb(struct vchiq_mmal_instance *instance,
1150                          struct vchiq_mmal_port *port, int status,
1151                          struct mmal_buffer *mmal_buf)
1152 {
1153         struct bcm2835_codec_ctx *ctx = port->cb_ctx;
1154         enum vb2_buffer_state buf_state = VB2_BUF_STATE_DONE;
1155         struct m2m_mmal_buffer *buf;
1156         struct vb2_v4l2_buffer *vb2;
1157
1158         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev,
1159                  "%s: status:%d, buf:%p, length:%lu, flags %04x, pts %lld\n",
1160                  __func__, status, mmal_buf, mmal_buf->length,
1161                  mmal_buf->mmal_flags, mmal_buf->pts);
1162
1163         buf = container_of(mmal_buf, struct m2m_mmal_buffer, mmal);
1164         vb2 = &buf->m2m.vb;
1165
1166         if (status) {
1167                 /* error in transfer */
1168                 if (vb2) {
1169                         /* there was a buffer with the error so return it */
1170                         vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_ERROR);
1171                 }
1172                 return;
1173         }
1174
1175         if (mmal_buf->cmd) {
1176                 switch (mmal_buf->cmd) {
1177                 case MMAL_EVENT_FORMAT_CHANGED:
1178                 {
1179                         handle_fmt_changed(ctx, mmal_buf);
1180                         break;
1181                 }
1182                 default:
1183                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Unexpected event on output callback - %08x\n",
1184                                  __func__, mmal_buf->cmd);
1185                         break;
1186                 }
1187                 return;
1188         }
1189
1190         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: length %lu, flags %x, idx %u\n",
1191                  __func__, mmal_buf->length, mmal_buf->mmal_flags,
1192                  vb2->vb2_buf.index);
1193
1194         if (mmal_buf->length == 0) {
1195                 /* stream ended, or buffer being returned during disable. */
1196                 v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: Empty buffer - flags %04x",
1197                          __func__, mmal_buf->mmal_flags);
1198                 if (!(mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS)) {
1199                         if (!port->enabled) {
1200                                 vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_QUEUED);
1201                                 if (atomic_read(&port->buffers_with_vpu))
1202                                         complete(&ctx->frame_cmplt);
1203                         } else {
1204                                 vchiq_mmal_submit_buffer(ctx->dev->instance,
1205                                                          &ctx->component->output[0],
1206                                                          mmal_buf);
1207                         }
1208                         return;
1209                 }
1210         }
1211         if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS) {
1212                 /* EOS packet from the VPU */
1213                 send_eos_event(ctx);
1214                 vb2->flags |= V4L2_BUF_FLAG_LAST;
1215         }
1216
1217         if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_CORRUPTED)
1218                 buf_state = VB2_BUF_STATE_ERROR;
1219
1220         /* vb2 timestamps in nsecs, mmal in usecs */
1221         vb2->vb2_buf.timestamp = mmal_buf->pts * 1000;
1222
1223         vb2_set_plane_payload(&vb2->vb2_buf, 0, mmal_buf->length);
1224         switch (mmal_buf->mmal_flags &
1225                                 (MMAL_BUFFER_HEADER_VIDEO_FLAG_INTERLACED |
1226                                  MMAL_BUFFER_HEADER_VIDEO_FLAG_TOP_FIELD_FIRST)) {
1227         case 0:
1228         case MMAL_BUFFER_HEADER_VIDEO_FLAG_TOP_FIELD_FIRST: /* Bogus */
1229                 vb2->field = V4L2_FIELD_NONE;
1230                 break;
1231         case MMAL_BUFFER_HEADER_VIDEO_FLAG_INTERLACED:
1232                 vb2->field = V4L2_FIELD_INTERLACED_BT;
1233                 break;
1234         case (MMAL_BUFFER_HEADER_VIDEO_FLAG_INTERLACED |
1235               MMAL_BUFFER_HEADER_VIDEO_FLAG_TOP_FIELD_FIRST):
1236                 vb2->field = V4L2_FIELD_INTERLACED_TB;
1237                 break;
1238         }
1239
1240         if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
1241                 vb2->flags |= V4L2_BUF_FLAG_KEYFRAME;
1242
1243         vb2_buffer_done(&vb2->vb2_buf, buf_state);
1244         ctx->num_op_buffers++;
1245
1246         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: done %d output buffers\n",
1247                  __func__, ctx->num_op_buffers);
1248
1249         if (!port->enabled && atomic_read(&port->buffers_with_vpu))
1250                 complete(&ctx->frame_cmplt);
1251 }
1252
1253 /* vb2_to_mmal_buffer() - converts vb2 buffer header to MMAL
1254  *
1255  * Copies all the required fields from a VB2 buffer to the MMAL buffer header,
1256  * ready for sending to the VPU.
1257  */
1258 static void vb2_to_mmal_buffer(struct m2m_mmal_buffer *buf,
1259                                struct vb2_v4l2_buffer *vb2)
1260 {
1261         u64 pts;
1262
1263         buf->mmal.mmal_flags = 0;
1264         if (vb2->flags & V4L2_BUF_FLAG_KEYFRAME)
1265                 buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_FLAG_KEYFRAME;
1266
1267         /*
1268          * Adding this means that the data must be framed correctly as one frame
1269          * per buffer. The underlying decoder has no such requirement, but it
1270          * will reduce latency as the bistream parser will be kicked immediately
1271          * to parse the frame, rather than relying on its own heuristics for
1272          * when to wake up.
1273          */
1274         buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_FLAG_FRAME_END;
1275
1276         buf->mmal.length = vb2->vb2_buf.planes[0].bytesused;
1277         /*
1278          * Minor ambiguity in the V4L2 spec as to whether passing in a 0 length
1279          * buffer, or one with V4L2_BUF_FLAG_LAST set denotes end of stream.
1280          * Handle either.
1281          */
1282         if (!buf->mmal.length || vb2->flags & V4L2_BUF_FLAG_LAST)
1283                 buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_FLAG_EOS;
1284
1285         /* vb2 timestamps in nsecs, mmal in usecs */
1286         pts = vb2->vb2_buf.timestamp;
1287         do_div(pts, 1000);
1288         buf->mmal.pts = pts;
1289         buf->mmal.dts = MMAL_TIME_UNKNOWN;
1290
1291         switch (field_override ? field_override : vb2->field) {
1292         default:
1293         case V4L2_FIELD_NONE:
1294                 break;
1295         case V4L2_FIELD_INTERLACED_BT:
1296                 buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_VIDEO_FLAG_INTERLACED;
1297                 break;
1298         case V4L2_FIELD_INTERLACED_TB:
1299                 buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_VIDEO_FLAG_INTERLACED |
1300                                         MMAL_BUFFER_HEADER_VIDEO_FLAG_TOP_FIELD_FIRST;
1301                 break;
1302         }
1303 }
1304
1305 /* device_run() - prepares and starts the device
1306  *
1307  * This simulates all the immediate preparations required before starting
1308  * a device. This will be called by the framework when it decides to schedule
1309  * a particular instance.
1310  */
1311 static void device_run(void *priv)
1312 {
1313         struct bcm2835_codec_ctx *ctx = priv;
1314         struct bcm2835_codec_dev *dev = ctx->dev;
1315         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1316         struct m2m_mmal_buffer *src_m2m_buf = NULL, *dst_m2m_buf = NULL;
1317         struct v4l2_m2m_buffer *m2m;
1318         int ret;
1319
1320         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: off we go\n", __func__);
1321
1322         if (ctx->fh.m2m_ctx->out_q_ctx.q.streaming) {
1323                 src_buf = v4l2_m2m_buf_remove(&ctx->fh.m2m_ctx->out_q_ctx);
1324                 if (src_buf) {
1325                         m2m = container_of(src_buf, struct v4l2_m2m_buffer, vb);
1326                         src_m2m_buf = container_of(m2m, struct m2m_mmal_buffer,
1327                                                    m2m);
1328                         vb2_to_mmal_buffer(src_m2m_buf, src_buf);
1329
1330                         ret = vchiq_mmal_submit_buffer(dev->instance,
1331                                                        &ctx->component->input[0],
1332                                                        &src_m2m_buf->mmal);
1333                         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev,
1334                                  "%s: Submitted ip buffer len %lu, pts %llu, flags %04x\n",
1335                                  __func__, src_m2m_buf->mmal.length,
1336                                  src_m2m_buf->mmal.pts,
1337                                  src_m2m_buf->mmal.mmal_flags);
1338                         if (ret)
1339                                 v4l2_err(&ctx->dev->v4l2_dev,
1340                                          "%s: Failed submitting ip buffer\n",
1341                                          __func__);
1342                 }
1343         }
1344
1345         if (ctx->fh.m2m_ctx->cap_q_ctx.q.streaming) {
1346                 dst_buf = v4l2_m2m_buf_remove(&ctx->fh.m2m_ctx->cap_q_ctx);
1347                 if (dst_buf) {
1348                         m2m = container_of(dst_buf, struct v4l2_m2m_buffer, vb);
1349                         dst_m2m_buf = container_of(m2m, struct m2m_mmal_buffer,
1350                                                    m2m);
1351                         vb2_to_mmal_buffer(dst_m2m_buf, dst_buf);
1352
1353                         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev,
1354                                  "%s: Submitted op buffer\n", __func__);
1355                         ret = vchiq_mmal_submit_buffer(dev->instance,
1356                                                        &ctx->component->output[0],
1357                                                        &dst_m2m_buf->mmal);
1358                         if (ret)
1359                                 v4l2_err(&ctx->dev->v4l2_dev,
1360                                          "%s: Failed submitting op buffer\n",
1361                                          __func__);
1362                 }
1363         }
1364
1365         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: Submitted src %p, dst %p\n",
1366                  __func__, src_m2m_buf, dst_m2m_buf);
1367
1368         /* Complete the job here. */
1369         v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
1370 }
1371
1372 /*
1373  * video ioctls
1374  */
1375 static int vidioc_querycap(struct file *file, void *priv,
1376                            struct v4l2_capability *cap)
1377 {
1378         struct bcm2835_codec_dev *dev = video_drvdata(file);
1379
1380         strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
1381         strncpy(cap->card, dev->vfd.name, sizeof(cap->card) - 1);
1382         snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
1383                  MEM2MEM_NAME);
1384         return 0;
1385 }
1386
1387 static int enum_fmt(struct v4l2_fmtdesc *f, struct bcm2835_codec_ctx *ctx,
1388                     bool capture)
1389 {
1390         struct bcm2835_codec_fmt *fmt;
1391         struct bcm2835_codec_fmt_list *fmts =
1392                                         get_format_list(ctx->dev, capture);
1393
1394         if (f->index < fmts->num_entries) {
1395                 /* Format found */
1396                 fmt = &fmts->list[f->index];
1397                 f->pixelformat = fmt->fourcc;
1398                 f->flags = fmt->flags;
1399                 return 0;
1400         }
1401
1402         /* Format not found */
1403         return -EINVAL;
1404 }
1405
1406 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
1407                                    struct v4l2_fmtdesc *f)
1408 {
1409         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1410
1411         return enum_fmt(f, ctx, true);
1412 }
1413
1414 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
1415                                    struct v4l2_fmtdesc *f)
1416 {
1417         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1418
1419         return enum_fmt(f, ctx, false);
1420 }
1421
1422 static int vidioc_g_fmt(struct bcm2835_codec_ctx *ctx, struct v4l2_format *f)
1423 {
1424         struct vb2_queue *vq;
1425         struct bcm2835_codec_q_data *q_data;
1426
1427         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
1428         if (!vq)
1429                 return -EINVAL;
1430
1431         q_data = get_q_data(ctx, f->type);
1432
1433         f->fmt.pix_mp.width                     = q_data->crop_width;
1434         f->fmt.pix_mp.height                    = q_data->height;
1435         f->fmt.pix_mp.pixelformat               = q_data->fmt->fourcc;
1436         f->fmt.pix_mp.field                     = q_data->field;
1437         f->fmt.pix_mp.colorspace                = ctx->colorspace;
1438         f->fmt.pix_mp.plane_fmt[0].sizeimage    = q_data->sizeimage;
1439         f->fmt.pix_mp.plane_fmt[0].bytesperline = q_data->bytesperline;
1440         f->fmt.pix_mp.num_planes                = 1;
1441         f->fmt.pix_mp.ycbcr_enc                 = ctx->ycbcr_enc;
1442         f->fmt.pix_mp.quantization              = ctx->quant;
1443         f->fmt.pix_mp.xfer_func                 = ctx->xfer_func;
1444
1445         memset(f->fmt.pix_mp.plane_fmt[0].reserved, 0,
1446                sizeof(f->fmt.pix_mp.plane_fmt[0].reserved));
1447
1448         return 0;
1449 }
1450
1451 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
1452                                 struct v4l2_format *f)
1453 {
1454         return vidioc_g_fmt(file2ctx(file), f);
1455 }
1456
1457 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
1458                                 struct v4l2_format *f)
1459 {
1460         return vidioc_g_fmt(file2ctx(file), f);
1461 }
1462
1463 static int vidioc_try_fmt(struct bcm2835_codec_ctx *ctx, struct v4l2_format *f,
1464                           struct bcm2835_codec_fmt *fmt)
1465 {
1466         unsigned int sizeimage, min_bytesperline;
1467
1468         /*
1469          * The V4L2 specification requires the driver to correct the format
1470          * struct if any of the dimensions is unsupported
1471          */
1472         if (f->fmt.pix_mp.width > MAX_W)
1473                 f->fmt.pix_mp.width = MAX_W;
1474         if (f->fmt.pix_mp.height > MAX_H)
1475                 f->fmt.pix_mp.height = MAX_H;
1476
1477         if (!(fmt->flags & V4L2_FMT_FLAG_COMPRESSED)) {
1478                 /* Only clip min w/h on capture. Treat 0x0 as unknown. */
1479                 if (f->fmt.pix_mp.width < MIN_W)
1480                         f->fmt.pix_mp.width = MIN_W;
1481                 if (f->fmt.pix_mp.height < MIN_H)
1482                         f->fmt.pix_mp.height = MIN_H;
1483
1484                 /*
1485                  * For decoders and image encoders the buffer must have
1486                  * a vertical alignment of 16 lines.
1487                  * The selection will reflect any cropping rectangle when only
1488                  * some of the pixels are active.
1489                  */
1490                 if (ctx->dev->role == DECODE || ctx->dev->role == ENCODE_IMAGE)
1491                         f->fmt.pix_mp.height = ALIGN(f->fmt.pix_mp.height, 16);
1492         }
1493         f->fmt.pix_mp.num_planes = 1;
1494         min_bytesperline = get_bytesperline(f->fmt.pix_mp.width,
1495                                             f->fmt.pix_mp.height,
1496                                             fmt, ctx->dev->role);
1497         if (f->fmt.pix_mp.plane_fmt[0].bytesperline < min_bytesperline)
1498                 f->fmt.pix_mp.plane_fmt[0].bytesperline = min_bytesperline;
1499         f->fmt.pix_mp.plane_fmt[0].bytesperline =
1500                 ALIGN(f->fmt.pix_mp.plane_fmt[0].bytesperline,
1501                       fmt->bytesperline_align[ctx->dev->role]);
1502
1503         sizeimage = get_sizeimage(f->fmt.pix_mp.plane_fmt[0].bytesperline,
1504                                   f->fmt.pix_mp.width, f->fmt.pix_mp.height,
1505                                   fmt);
1506         /*
1507          * Drivers must set sizeimage for uncompressed formats
1508          * Compressed formats allow the client to request an alternate
1509          * size for the buffer.
1510          */
1511         if (!(fmt->flags & V4L2_FMT_FLAG_COMPRESSED) ||
1512             f->fmt.pix_mp.plane_fmt[0].sizeimage < sizeimage)
1513                 f->fmt.pix_mp.plane_fmt[0].sizeimage = sizeimage;
1514
1515         memset(f->fmt.pix_mp.plane_fmt[0].reserved, 0,
1516                sizeof(f->fmt.pix_mp.plane_fmt[0].reserved));
1517
1518         if (ctx->dev->role == DECODE || ctx->dev->role == DEINTERLACE) {
1519                 switch (f->fmt.pix_mp.field) {
1520                 /*
1521                  * All of this is pretty much guesswork as we'll set the
1522                  * interlace format correctly come format changed, and signal
1523                  * it appropriately on each buffer.
1524                  */
1525                 default:
1526                 case V4L2_FIELD_NONE:
1527                 case V4L2_FIELD_ANY:
1528                         f->fmt.pix_mp.field = V4L2_FIELD_NONE;
1529                         break;
1530                 case V4L2_FIELD_INTERLACED:
1531                         f->fmt.pix_mp.field = V4L2_FIELD_INTERLACED;
1532                         break;
1533                 case V4L2_FIELD_TOP:
1534                 case V4L2_FIELD_BOTTOM:
1535                 case V4L2_FIELD_INTERLACED_TB:
1536                         f->fmt.pix_mp.field = V4L2_FIELD_INTERLACED_TB;
1537                         break;
1538                 case V4L2_FIELD_INTERLACED_BT:
1539                         f->fmt.pix_mp.field = V4L2_FIELD_INTERLACED_BT;
1540                         break;
1541                 }
1542         } else {
1543                 f->fmt.pix_mp.field = V4L2_FIELD_NONE;
1544         }
1545
1546         return 0;
1547 }
1548
1549 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
1550                                   struct v4l2_format *f)
1551 {
1552         struct bcm2835_codec_fmt *fmt;
1553         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1554
1555         fmt = find_format(f, ctx->dev, true);
1556         if (!fmt) {
1557                 f->fmt.pix_mp.pixelformat = get_default_format(ctx->dev,
1558                                                                true)->fourcc;
1559                 fmt = find_format(f, ctx->dev, true);
1560         }
1561
1562         return vidioc_try_fmt(ctx, f, fmt);
1563 }
1564
1565 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
1566                                   struct v4l2_format *f)
1567 {
1568         struct bcm2835_codec_fmt *fmt;
1569         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1570
1571         fmt = find_format(f, ctx->dev, false);
1572         if (!fmt) {
1573                 f->fmt.pix_mp.pixelformat = get_default_format(ctx->dev,
1574                                                                false)->fourcc;
1575                 fmt = find_format(f, ctx->dev, false);
1576         }
1577
1578         if (!f->fmt.pix_mp.colorspace)
1579                 f->fmt.pix_mp.colorspace = ctx->colorspace;
1580
1581         return vidioc_try_fmt(ctx, f, fmt);
1582 }
1583
1584 static int vidioc_s_fmt(struct bcm2835_codec_ctx *ctx, struct v4l2_format *f,
1585                         unsigned int requested_height)
1586 {
1587         struct bcm2835_codec_q_data *q_data;
1588         struct vb2_queue *vq;
1589         struct vchiq_mmal_port *port;
1590         bool update_capture_port = false;
1591         bool reenable_port = false;
1592         int ret;
1593
1594         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Setting format for type %d, wxh: %dx%d, fmt: %08x, size %u\n",
1595                  f->type, f->fmt.pix_mp.width, f->fmt.pix_mp.height,
1596                  f->fmt.pix_mp.pixelformat,
1597                  f->fmt.pix_mp.plane_fmt[0].sizeimage);
1598
1599         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
1600         if (!vq)
1601                 return -EINVAL;
1602
1603         q_data = get_q_data(ctx, f->type);
1604         if (!q_data)
1605                 return -EINVAL;
1606
1607         if (vb2_is_busy(vq)) {
1608                 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
1609                 return -EBUSY;
1610         }
1611
1612         q_data->fmt = find_format(f, ctx->dev,
1613                                   f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
1614         q_data->crop_width = f->fmt.pix_mp.width;
1615         q_data->height = f->fmt.pix_mp.height;
1616         if (!q_data->selection_set ||
1617             (q_data->fmt->flags & V4L2_FMT_FLAG_COMPRESSED))
1618                 q_data->crop_height = requested_height;
1619
1620         /*
1621          * Copying the behaviour of vicodec which retains a single set of
1622          * colorspace parameters for both input and output.
1623          */
1624         ctx->colorspace = f->fmt.pix_mp.colorspace;
1625         ctx->xfer_func = f->fmt.pix_mp.xfer_func;
1626         ctx->ycbcr_enc = f->fmt.pix_mp.ycbcr_enc;
1627         ctx->quant = f->fmt.pix_mp.quantization;
1628
1629         q_data->field = f->fmt.pix_mp.field;
1630
1631         /* All parameters should have been set correctly by try_fmt */
1632         q_data->bytesperline = f->fmt.pix_mp.plane_fmt[0].bytesperline;
1633         q_data->sizeimage = f->fmt.pix_mp.plane_fmt[0].sizeimage;
1634
1635         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Calculated bpl as %u, size %u\n",
1636                  q_data->bytesperline, q_data->sizeimage);
1637
1638         if ((ctx->dev->role == DECODE || ctx->dev->role == ENCODE_IMAGE) &&
1639             q_data->fmt->flags & V4L2_FMT_FLAG_COMPRESSED &&
1640             q_data->crop_width && q_data->height) {
1641                 /*
1642                  * On the decoder or image encoder, if provided with
1643                  * a resolution on the input side, then replicate that
1644                  * to the output side.
1645                  * GStreamer appears not to support V4L2_EVENT_SOURCE_CHANGE,
1646                  * nor set up a resolution on the output side, therefore
1647                  * we can't decode anything at a resolution other than the
1648                  * default one.
1649                  */
1650                 struct bcm2835_codec_q_data *q_data_dst =
1651                                                 &ctx->q_data[V4L2_M2M_DST];
1652
1653                 q_data_dst->crop_width = q_data->crop_width;
1654                 q_data_dst->crop_height = q_data->crop_height;
1655                 q_data_dst->height = ALIGN(q_data->crop_height, 16);
1656
1657                 q_data_dst->bytesperline =
1658                         get_bytesperline(f->fmt.pix_mp.width,
1659                                          f->fmt.pix_mp.height,
1660                                          q_data_dst->fmt, ctx->dev->role);
1661                 q_data_dst->sizeimage = get_sizeimage(q_data_dst->bytesperline,
1662                                                       q_data_dst->crop_width,
1663                                                       q_data_dst->height,
1664                                                       q_data_dst->fmt);
1665                 update_capture_port = true;
1666         }
1667
1668         /* If we have a component then setup the port as well */
1669         port = get_port_data(ctx, vq->type);
1670         if (!port)
1671                 return 0;
1672
1673         if (port->enabled) {
1674                 unsigned int num_buffers;
1675
1676                 /*
1677                  * This should only ever happen with DECODE and the MMAL output
1678                  * port that has been enabled for resolution changed events.
1679                  * In this case no buffers have been allocated or sent to the
1680                  * component, so warn on that.
1681                  */
1682                 WARN_ON(ctx->dev->role != DECODE ||
1683                         f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ||
1684                         atomic_read(&port->buffers_with_vpu));
1685
1686                 /*
1687                  * Disable will reread the port format, so retain buffer count.
1688                  */
1689                 num_buffers = port->current_buffer.num;
1690
1691                 ret = vchiq_mmal_port_disable(ctx->dev->instance, port);
1692                 if (ret)
1693                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Error disabling port update buffer count, ret %d\n",
1694                                  __func__, ret);
1695
1696                 port->current_buffer.num = num_buffers;
1697
1698                 reenable_port = true;
1699         }
1700
1701         setup_mmal_port_format(ctx, q_data, port);
1702         ret = vchiq_mmal_port_set_format(ctx->dev->instance, port);
1703         if (ret) {
1704                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed vchiq_mmal_port_set_format on port, ret %d\n",
1705                          __func__, ret);
1706                 ret = -EINVAL;
1707         }
1708
1709         if (q_data->sizeimage < port->minimum_buffer.size) {
1710                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Current buffer size of %u < min buf size %u - driver mismatch to MMAL\n",
1711                          __func__, q_data->sizeimage,
1712                          port->minimum_buffer.size);
1713         }
1714
1715         if (reenable_port) {
1716                 ret = vchiq_mmal_port_enable(ctx->dev->instance,
1717                                              port,
1718                                              op_buffer_cb);
1719                 if (ret)
1720                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling o/p port, ret %d\n",
1721                                  __func__, ret);
1722         }
1723         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Set format for type %d, wxh: %dx%d, fmt: %08x, size %u\n",
1724                  f->type, q_data->crop_width, q_data->height,
1725                  q_data->fmt->fourcc, q_data->sizeimage);
1726
1727         if (update_capture_port) {
1728                 struct vchiq_mmal_port *port_dst = &ctx->component->output[0];
1729                 struct bcm2835_codec_q_data *q_data_dst =
1730                                                 &ctx->q_data[V4L2_M2M_DST];
1731
1732                 setup_mmal_port_format(ctx, q_data_dst, port_dst);
1733                 ret = vchiq_mmal_port_set_format(ctx->dev->instance, port_dst);
1734                 if (ret) {
1735                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed vchiq_mmal_port_set_format on output port, ret %d\n",
1736                                  __func__, ret);
1737                         ret = -EINVAL;
1738                 }
1739         }
1740         return ret;
1741 }
1742
1743 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
1744                                 struct v4l2_format *f)
1745 {
1746         unsigned int height = f->fmt.pix_mp.height;
1747         int ret;
1748
1749         ret = vidioc_try_fmt_vid_cap(file, priv, f);
1750         if (ret)
1751                 return ret;
1752
1753         return vidioc_s_fmt(file2ctx(file), f, height);
1754 }
1755
1756 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
1757                                 struct v4l2_format *f)
1758 {
1759         unsigned int height = f->fmt.pix_mp.height;
1760         int ret;
1761
1762         ret = vidioc_try_fmt_vid_out(file, priv, f);
1763         if (ret)
1764                 return ret;
1765
1766         ret = vidioc_s_fmt(file2ctx(file), f, height);
1767         return ret;
1768 }
1769
1770 static int vidioc_g_selection(struct file *file, void *priv,
1771                               struct v4l2_selection *s)
1772 {
1773         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1774         struct bcm2835_codec_q_data *q_data;
1775
1776         /*
1777          * The selection API takes V4L2_BUF_TYPE_VIDEO_CAPTURE and
1778          * V4L2_BUF_TYPE_VIDEO_OUTPUT, even if the device implements the MPLANE
1779          * API. The V4L2 core will have converted the MPLANE variants to
1780          * non-MPLANE.
1781          * Open code this instead of using get_q_data in this case.
1782          */
1783         switch (s->type) {
1784         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1785                 /* CAPTURE on encoder is not valid. */
1786                 if (ctx->dev->role == ENCODE || ctx->dev->role == ENCODE_IMAGE)
1787                         return -EINVAL;
1788                 q_data = &ctx->q_data[V4L2_M2M_DST];
1789                 break;
1790         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1791                 /* OUTPUT on deoder is not valid. */
1792                 if (ctx->dev->role == DECODE)
1793                         return -EINVAL;
1794                 q_data = &ctx->q_data[V4L2_M2M_SRC];
1795                 break;
1796         default:
1797                 return -EINVAL;
1798         }
1799
1800         switch (ctx->dev->role) {
1801         case DECODE:
1802                 switch (s->target) {
1803                 case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1804                 case V4L2_SEL_TGT_COMPOSE:
1805                         s->r.left = 0;
1806                         s->r.top = 0;
1807                         s->r.width = q_data->crop_width;
1808                         s->r.height = q_data->crop_height;
1809                         break;
1810                 case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1811                         s->r.left = 0;
1812                         s->r.top = 0;
1813                         s->r.width = q_data->crop_width;
1814                         s->r.height = q_data->crop_height;
1815                         break;
1816                 case V4L2_SEL_TGT_CROP_BOUNDS:
1817                 case V4L2_SEL_TGT_CROP_DEFAULT:
1818                         s->r.left = 0;
1819                         s->r.top = 0;
1820                         s->r.width = (q_data->bytesperline << 3) /
1821                                                 q_data->fmt->depth;
1822                         s->r.height = q_data->height;
1823                         break;
1824                 default:
1825                         return -EINVAL;
1826                 }
1827                 break;
1828         case ENCODE:
1829         case ENCODE_IMAGE:
1830                 switch (s->target) {
1831                 case V4L2_SEL_TGT_CROP_DEFAULT:
1832                 case V4L2_SEL_TGT_CROP_BOUNDS:
1833                         s->r.top = 0;
1834                         s->r.left = 0;
1835                         s->r.width = q_data->bytesperline;
1836                         s->r.height = q_data->height;
1837                         break;
1838                 case V4L2_SEL_TGT_CROP:
1839                         s->r.top = 0;
1840                         s->r.left = 0;
1841                         s->r.width = q_data->crop_width;
1842                         s->r.height = q_data->crop_height;
1843                         break;
1844                 default:
1845                         return -EINVAL;
1846                 }
1847                 break;
1848         case ISP:
1849                 break;
1850         case DEINTERLACE:
1851                 if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
1852                         switch (s->target) {
1853                         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1854                         case V4L2_SEL_TGT_COMPOSE:
1855                                 s->r.left = 0;
1856                                 s->r.top = 0;
1857                                 s->r.width = q_data->crop_width;
1858                                 s->r.height = q_data->crop_height;
1859                                 break;
1860                         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1861                                 s->r.left = 0;
1862                                 s->r.top = 0;
1863                                 s->r.width = q_data->crop_width;
1864                                 s->r.height = q_data->crop_height;
1865                                 break;
1866                         default:
1867                                 return -EINVAL;
1868                         }
1869                 } else {
1870                         /* must be V4L2_BUF_TYPE_VIDEO_OUTPUT */
1871                         switch (s->target) {
1872                         case V4L2_SEL_TGT_CROP_DEFAULT:
1873                         case V4L2_SEL_TGT_CROP_BOUNDS:
1874                                 s->r.top = 0;
1875                                 s->r.left = 0;
1876                                 s->r.width = q_data->bytesperline;
1877                                 s->r.height = q_data->height;
1878                                 break;
1879                         case V4L2_SEL_TGT_CROP:
1880                                 s->r.top = 0;
1881                                 s->r.left = 0;
1882                                 s->r.width = q_data->crop_width;
1883                                 s->r.height = q_data->crop_height;
1884                                 break;
1885                         default:
1886                                 return -EINVAL;
1887                         }
1888                 }
1889                 break;
1890         case NUM_ROLES:
1891                 break;
1892         }
1893
1894         return 0;
1895 }
1896
1897 static int vidioc_s_selection(struct file *file, void *priv,
1898                               struct v4l2_selection *s)
1899 {
1900         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1901         struct bcm2835_codec_q_data *q_data = NULL;
1902         struct vchiq_mmal_port *port = NULL;
1903         int ret;
1904
1905         /*
1906          * The selection API takes V4L2_BUF_TYPE_VIDEO_CAPTURE and
1907          * V4L2_BUF_TYPE_VIDEO_OUTPUT, even if the device implements the MPLANE
1908          * API. The V4L2 core will have converted the MPLANE variants to
1909          * non-MPLANE.
1910          *
1911          * Open code this instead of using get_q_data in this case.
1912          */
1913         switch (s->type) {
1914         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1915                 /* CAPTURE on encoder is not valid. */
1916                 if (ctx->dev->role == ENCODE || ctx->dev->role == ENCODE_IMAGE)
1917                         return -EINVAL;
1918                 q_data = &ctx->q_data[V4L2_M2M_DST];
1919                 if (ctx->component)
1920                         port = &ctx->component->output[0];
1921                 break;
1922         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1923                 /* OUTPUT on deoder is not valid. */
1924                 if (ctx->dev->role == DECODE)
1925                         return -EINVAL;
1926                 q_data = &ctx->q_data[V4L2_M2M_SRC];
1927                 if (ctx->component)
1928                         port = &ctx->component->input[0];
1929                 break;
1930         default:
1931                 return -EINVAL;
1932         }
1933
1934         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",
1935                  __func__, ctx, s->type, q_data, s->target, s->r.left, s->r.top,
1936                  s->r.width, s->r.height);
1937
1938         switch (ctx->dev->role) {
1939         case DECODE:
1940                 switch (s->target) {
1941                 case V4L2_SEL_TGT_COMPOSE:
1942                         /* Accept cropped image */
1943                         s->r.left = 0;
1944                         s->r.top = 0;
1945                         s->r.width = min(s->r.width, q_data->crop_width);
1946                         s->r.height = min(s->r.height, q_data->height);
1947                         q_data->crop_width = s->r.width;
1948                         q_data->crop_height = s->r.height;
1949                         q_data->selection_set = true;
1950                         break;
1951                 default:
1952                         return -EINVAL;
1953                 }
1954                 break;
1955         case ENCODE:
1956         case ENCODE_IMAGE:
1957                 switch (s->target) {
1958                 case V4L2_SEL_TGT_CROP:
1959                         /* Only support crop from (0,0) */
1960                         s->r.top = 0;
1961                         s->r.left = 0;
1962                         s->r.width = min(s->r.width, q_data->crop_width);
1963                         s->r.height = min(s->r.height, q_data->height);
1964                         q_data->crop_width = s->r.width;
1965                         q_data->crop_height = s->r.height;
1966                         q_data->selection_set = true;
1967                         break;
1968                 default:
1969                         return -EINVAL;
1970                 }
1971                 break;
1972         case ISP:
1973                 break;
1974         case DEINTERLACE:
1975                 if (s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
1976                         switch (s->target) {
1977                         case V4L2_SEL_TGT_COMPOSE:
1978                                 /* Accept cropped image */
1979                                 s->r.left = 0;
1980                                 s->r.top = 0;
1981                                 s->r.width = min(s->r.width, q_data->crop_width);
1982                                 s->r.height = min(s->r.height, q_data->height);
1983                                 q_data->crop_width = s->r.width;
1984                                 q_data->crop_height = s->r.height;
1985                                 q_data->selection_set = true;
1986                                 break;
1987                         default:
1988                                 return -EINVAL;
1989                         }
1990                         break;
1991                 } else {
1992                         /* must be V4L2_BUF_TYPE_VIDEO_OUTPUT */
1993                         switch (s->target) {
1994                         case V4L2_SEL_TGT_CROP:
1995                                 /* Only support crop from (0,0) */
1996                                 s->r.top = 0;
1997                                 s->r.left = 0;
1998                                 s->r.width = min(s->r.width, q_data->crop_width);
1999                                 s->r.height = min(s->r.height, q_data->height);
2000                                 q_data->crop_width = s->r.width;
2001                                 q_data->crop_height = s->r.height;
2002                                 q_data->selection_set = true;
2003                                 break;
2004                         default:
2005                                 return -EINVAL;
2006                         }
2007                         break;
2008                 }
2009         case NUM_ROLES:
2010                 break;
2011         }
2012
2013         if (!port)
2014                 return 0;
2015
2016         setup_mmal_port_format(ctx, q_data, port);
2017         ret = vchiq_mmal_port_set_format(ctx->dev->instance, port);
2018         if (ret) {
2019                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed vchiq_mmal_port_set_format on port, ret %d\n",
2020                          __func__, ret);
2021                 return -EINVAL;
2022         }
2023
2024         return 0;
2025 }
2026
2027 static int vidioc_s_parm(struct file *file, void *priv,
2028                          struct v4l2_streamparm *parm)
2029 {
2030         struct bcm2835_codec_ctx *ctx = file2ctx(file);
2031
2032         if (parm->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
2033                 return -EINVAL;
2034
2035         if (!parm->parm.output.timeperframe.denominator ||
2036             !parm->parm.output.timeperframe.numerator)
2037                 return -EINVAL;
2038
2039         ctx->framerate_num =
2040                         parm->parm.output.timeperframe.denominator;
2041         ctx->framerate_denom =
2042                         parm->parm.output.timeperframe.numerator;
2043
2044         parm->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
2045
2046         return 0;
2047 }
2048
2049 static int vidioc_g_parm(struct file *file, void *priv,
2050                          struct v4l2_streamparm *parm)
2051 {
2052         struct bcm2835_codec_ctx *ctx = file2ctx(file);
2053
2054         if (parm->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
2055                 return -EINVAL;
2056
2057         parm->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
2058         parm->parm.output.timeperframe.denominator =
2059                         ctx->framerate_num;
2060         parm->parm.output.timeperframe.numerator =
2061                         ctx->framerate_denom;
2062
2063         return 0;
2064 }
2065
2066 static int vidioc_g_pixelaspect(struct file *file, void *fh, int type,
2067                                 struct v4l2_fract *f)
2068 {
2069         struct bcm2835_codec_ctx *ctx = file2ctx(file);
2070
2071         /*
2072          * The selection API takes V4L2_BUF_TYPE_VIDEO_CAPTURE and
2073          * V4L2_BUF_TYPE_VIDEO_OUTPUT, even if the device implements the MPLANE
2074          * API. The V4L2 core will have converted the MPLANE variants to
2075          * non-MPLANE.
2076          * Open code this instead of using get_q_data in this case.
2077          */
2078         if (ctx->dev->role != DECODE)
2079                 return -ENOIOCTLCMD;
2080
2081         if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
2082                 return -EINVAL;
2083
2084         *f = ctx->q_data[V4L2_M2M_DST].aspect_ratio;
2085
2086         return 0;
2087 }
2088
2089 static int vidioc_subscribe_evt(struct v4l2_fh *fh,
2090                                 const struct v4l2_event_subscription *sub)
2091 {
2092         switch (sub->type) {
2093         case V4L2_EVENT_EOS:
2094                 return v4l2_event_subscribe(fh, sub, 2, NULL);
2095         case V4L2_EVENT_SOURCE_CHANGE:
2096                 return v4l2_src_change_event_subscribe(fh, sub);
2097         default:
2098                 return v4l2_ctrl_subscribe_event(fh, sub);
2099         }
2100 }
2101
2102 static int bcm2835_codec_set_level_profile(struct bcm2835_codec_ctx *ctx,
2103                                            struct v4l2_ctrl *ctrl)
2104 {
2105         struct mmal_parameter_video_profile param;
2106         int param_size = sizeof(param);
2107         int ret;
2108
2109         /*
2110          * Level and Profile are set via the same MMAL parameter.
2111          * Retrieve the current settings and amend the one that has changed.
2112          */
2113         ret = vchiq_mmal_port_parameter_get(ctx->dev->instance,
2114                                             &ctx->component->output[0],
2115                                             MMAL_PARAMETER_PROFILE,
2116                                             &param,
2117                                             &param_size);
2118         if (ret)
2119                 return ret;
2120
2121         switch (ctrl->id) {
2122         case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
2123                 switch (ctrl->val) {
2124                 case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
2125                         param.profile = MMAL_VIDEO_PROFILE_H264_BASELINE;
2126                         break;
2127                 case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
2128                         param.profile =
2129                                 MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE;
2130                         break;
2131                 case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
2132                         param.profile = MMAL_VIDEO_PROFILE_H264_MAIN;
2133                         break;
2134                 case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
2135                         param.profile = MMAL_VIDEO_PROFILE_H264_HIGH;
2136                         break;
2137                 default:
2138                         /* Should never get here */
2139                         break;
2140                 }
2141                 break;
2142
2143         case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
2144                 switch (ctrl->val) {
2145                 case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
2146                         param.level = MMAL_VIDEO_LEVEL_H264_1;
2147                         break;
2148                 case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
2149                         param.level = MMAL_VIDEO_LEVEL_H264_1b;
2150                         break;
2151                 case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
2152                         param.level = MMAL_VIDEO_LEVEL_H264_11;
2153                         break;
2154                 case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
2155                         param.level = MMAL_VIDEO_LEVEL_H264_12;
2156                         break;
2157                 case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
2158                         param.level = MMAL_VIDEO_LEVEL_H264_13;
2159                         break;
2160                 case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
2161                         param.level = MMAL_VIDEO_LEVEL_H264_2;
2162                         break;
2163                 case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
2164                         param.level = MMAL_VIDEO_LEVEL_H264_21;
2165                         break;
2166                 case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
2167                         param.level = MMAL_VIDEO_LEVEL_H264_22;
2168                         break;
2169                 case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
2170                         param.level = MMAL_VIDEO_LEVEL_H264_3;
2171                         break;
2172                 case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
2173                         param.level = MMAL_VIDEO_LEVEL_H264_31;
2174                         break;
2175                 case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
2176                         param.level = MMAL_VIDEO_LEVEL_H264_32;
2177                         break;
2178                 case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
2179                         param.level = MMAL_VIDEO_LEVEL_H264_4;
2180                         break;
2181                 /*
2182                  * Note that the hardware spec is level 4.0. Levels above that
2183                  * are there for correctly encoding the headers and may not
2184                  * be able to keep up with real-time.
2185                  */
2186                 case V4L2_MPEG_VIDEO_H264_LEVEL_4_1:
2187                         param.level = MMAL_VIDEO_LEVEL_H264_41;
2188                         break;
2189                 case V4L2_MPEG_VIDEO_H264_LEVEL_4_2:
2190                         param.level = MMAL_VIDEO_LEVEL_H264_42;
2191                         break;
2192                 case V4L2_MPEG_VIDEO_H264_LEVEL_5_0:
2193                         param.level = MMAL_VIDEO_LEVEL_H264_5;
2194                         break;
2195                 case V4L2_MPEG_VIDEO_H264_LEVEL_5_1:
2196                         param.level = MMAL_VIDEO_LEVEL_H264_51;
2197                         break;
2198                 default:
2199                         /* Should never get here */
2200                         break;
2201                 }
2202         }
2203         ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2204                                             &ctx->component->output[0],
2205                                             MMAL_PARAMETER_PROFILE,
2206                                             &param,
2207                                             param_size);
2208
2209         return ret;
2210 }
2211
2212 static int bcm2835_codec_s_ctrl(struct v4l2_ctrl *ctrl)
2213 {
2214         struct bcm2835_codec_ctx *ctx =
2215                 container_of(ctrl->handler, struct bcm2835_codec_ctx, hdl);
2216         int ret = 0;
2217
2218         switch (ctrl->id) {
2219         case V4L2_CID_MPEG_VIDEO_BITRATE:
2220                 ctx->bitrate = ctrl->val;
2221                 if (!ctx->component)
2222                         break;
2223
2224                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2225                                                     &ctx->component->output[0],
2226                                                     MMAL_PARAMETER_VIDEO_BIT_RATE,
2227                                                     &ctrl->val,
2228                                                     sizeof(ctrl->val));
2229                 break;
2230
2231         case V4L2_CID_MPEG_VIDEO_BITRATE_MODE: {
2232                 u32 bitrate_mode;
2233
2234                 if (!ctx->component)
2235                         break;
2236
2237                 switch (ctrl->val) {
2238                 default:
2239                 case V4L2_MPEG_VIDEO_BITRATE_MODE_VBR:
2240                         bitrate_mode = MMAL_VIDEO_RATECONTROL_VARIABLE;
2241                         break;
2242                 case V4L2_MPEG_VIDEO_BITRATE_MODE_CBR:
2243                         bitrate_mode = MMAL_VIDEO_RATECONTROL_CONSTANT;
2244                         break;
2245                 }
2246
2247                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2248                                                     &ctx->component->output[0],
2249                                                     MMAL_PARAMETER_RATECONTROL,
2250                                                     &bitrate_mode,
2251                                                     sizeof(bitrate_mode));
2252                 break;
2253         }
2254         case V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER:
2255                 if (!ctx->component)
2256                         break;
2257
2258                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2259                                                     &ctx->component->output[0],
2260                                                     MMAL_PARAMETER_VIDEO_ENCODE_INLINE_HEADER,
2261                                                     &ctrl->val,
2262                                                     sizeof(ctrl->val));
2263                 break;
2264
2265         case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
2266                 if (!ctx->component)
2267                         break;
2268
2269                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2270                                                     &ctx->component->output[0],
2271                                                     MMAL_PARAMETER_VIDEO_ENCODE_HEADERS_WITH_FRAME,
2272                                                     &ctrl->val,
2273                                                     sizeof(ctrl->val));
2274                 break;
2275
2276         case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD:
2277                 if (!ctx->component)
2278                         break;
2279
2280                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2281                                                     &ctx->component->output[0],
2282                                                     MMAL_PARAMETER_INTRAPERIOD,
2283                                                     &ctrl->val,
2284                                                     sizeof(ctrl->val));
2285                 break;
2286
2287         case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
2288         case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
2289                 if (!ctx->component)
2290                         break;
2291
2292                 ret = bcm2835_codec_set_level_profile(ctx, ctrl);
2293                 break;
2294
2295         case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
2296                 if (!ctx->component)
2297                         break;
2298
2299                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2300                                                     &ctx->component->output[0],
2301                                                     MMAL_PARAMETER_VIDEO_ENCODE_MIN_QUANT,
2302                                                     &ctrl->val,
2303                                                     sizeof(ctrl->val));
2304                 break;
2305
2306         case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
2307                 if (!ctx->component)
2308                         break;
2309
2310                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2311                                                     &ctx->component->output[0],
2312                                                     MMAL_PARAMETER_VIDEO_ENCODE_MAX_QUANT,
2313                                                     &ctrl->val,
2314                                                     sizeof(ctrl->val));
2315                 break;
2316
2317         case V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME: {
2318                 u32 mmal_bool = 1;
2319
2320                 if (!ctx->component)
2321                         break;
2322
2323                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2324                                                     &ctx->component->output[0],
2325                                                     MMAL_PARAMETER_VIDEO_REQUEST_I_FRAME,
2326                                                     &mmal_bool,
2327                                                     sizeof(mmal_bool));
2328                 break;
2329         }
2330         case V4L2_CID_HFLIP:
2331         case V4L2_CID_VFLIP: {
2332                 u32 u32_value;
2333
2334                 if (ctrl->id == V4L2_CID_HFLIP)
2335                         ctx->hflip = ctrl->val;
2336                 else
2337                         ctx->vflip = ctrl->val;
2338
2339                 if (!ctx->component)
2340                         break;
2341
2342                 if (ctx->hflip && ctx->vflip)
2343                         u32_value = MMAL_PARAM_MIRROR_BOTH;
2344                 else if (ctx->hflip)
2345                         u32_value = MMAL_PARAM_MIRROR_HORIZONTAL;
2346                 else if (ctx->vflip)
2347                         u32_value = MMAL_PARAM_MIRROR_VERTICAL;
2348                 else
2349                         u32_value = MMAL_PARAM_MIRROR_NONE;
2350
2351                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2352                                                     &ctx->component->input[0],
2353                                                     MMAL_PARAMETER_MIRROR,
2354                                                     &u32_value,
2355                                                     sizeof(u32_value));
2356                 break;
2357         }
2358         case V4L2_CID_JPEG_COMPRESSION_QUALITY:
2359                 if (!ctx->component)
2360                         break;
2361
2362                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
2363                                                     &ctx->component->output[0],
2364                                                     MMAL_PARAMETER_JPEG_Q_FACTOR,
2365                                                     &ctrl->val,
2366                                                     sizeof(ctrl->val));
2367                 break;
2368
2369         default:
2370                 v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
2371                 return -EINVAL;
2372         }
2373
2374         if (ret)
2375                 v4l2_err(&ctx->dev->v4l2_dev, "Failed setting ctrl %08x, ret %d\n",
2376                          ctrl->id, ret);
2377         return ret ? -EINVAL : 0;
2378 }
2379
2380 static const struct v4l2_ctrl_ops bcm2835_codec_ctrl_ops = {
2381         .s_ctrl = bcm2835_codec_s_ctrl,
2382 };
2383
2384 static int vidioc_try_decoder_cmd(struct file *file, void *priv,
2385                                   struct v4l2_decoder_cmd *cmd)
2386 {
2387         struct bcm2835_codec_ctx *ctx = file2ctx(file);
2388
2389         if (ctx->dev->role != DECODE)
2390                 return -EINVAL;
2391
2392         switch (cmd->cmd) {
2393         case V4L2_DEC_CMD_STOP:
2394                 if (cmd->flags & V4L2_DEC_CMD_STOP_TO_BLACK) {
2395                         v4l2_err(&ctx->dev->v4l2_dev, "%s: DEC cmd->flags=%u stop to black not supported",
2396                                  __func__, cmd->flags);
2397                         return -EINVAL;
2398                 }
2399                 break;
2400         case V4L2_DEC_CMD_START:
2401                 break;
2402         default:
2403                 return -EINVAL;
2404         }
2405         return 0;
2406 }
2407
2408 static int vidioc_decoder_cmd(struct file *file, void *priv,
2409                               struct v4l2_decoder_cmd *cmd)
2410 {
2411         struct bcm2835_codec_ctx *ctx = file2ctx(file);
2412         struct bcm2835_codec_q_data *q_data = &ctx->q_data[V4L2_M2M_SRC];
2413         struct vb2_queue *dst_vq;
2414         int ret;
2415
2416         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s, cmd %u", __func__,
2417                  cmd->cmd);
2418         ret = vidioc_try_decoder_cmd(file, priv, cmd);
2419         if (ret)
2420                 return ret;
2421
2422         switch (cmd->cmd) {
2423         case V4L2_DEC_CMD_STOP:
2424                 if (q_data->eos_buffer_in_use)
2425                         v4l2_err(&ctx->dev->v4l2_dev, "EOS buffers already in use\n");
2426                 q_data->eos_buffer_in_use = true;
2427
2428                 q_data->eos_buffer.mmal.buffer_size = 0;
2429                 q_data->eos_buffer.mmal.length = 0;
2430                 q_data->eos_buffer.mmal.mmal_flags =
2431                                                 MMAL_BUFFER_HEADER_FLAG_EOS;
2432                 q_data->eos_buffer.mmal.pts = 0;
2433                 q_data->eos_buffer.mmal.dts = 0;
2434
2435                 if (!ctx->component)
2436                         break;
2437
2438                 ret = vchiq_mmal_submit_buffer(ctx->dev->instance,
2439                                                &ctx->component->input[0],
2440                                                &q_data->eos_buffer.mmal);
2441                 if (ret)
2442                         v4l2_err(&ctx->dev->v4l2_dev,
2443                                  "%s: EOS buffer submit failed %d\n",
2444                                  __func__, ret);
2445
2446                 break;
2447
2448         case V4L2_DEC_CMD_START:
2449                 dst_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
2450                                          V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
2451                 vb2_clear_last_buffer_dequeued(dst_vq);
2452                 break;
2453
2454         default:
2455                 return -EINVAL;
2456         }
2457
2458         return 0;
2459 }
2460
2461 static int vidioc_try_encoder_cmd(struct file *file, void *priv,
2462                                   struct v4l2_encoder_cmd *cmd)
2463 {
2464         struct bcm2835_codec_ctx *ctx = file2ctx(file);
2465
2466         if (ctx->dev->role != ENCODE && ctx->dev->role != ENCODE_IMAGE)
2467                 return -EINVAL;
2468
2469         switch (cmd->cmd) {
2470         case V4L2_ENC_CMD_STOP:
2471                 break;
2472
2473         case V4L2_ENC_CMD_START:
2474                 /* Do we need to do anything here? */
2475                 break;
2476         default:
2477                 return -EINVAL;
2478         }
2479         return 0;
2480 }
2481
2482 static int vidioc_encoder_cmd(struct file *file, void *priv,
2483                               struct v4l2_encoder_cmd *cmd)
2484 {
2485         struct bcm2835_codec_ctx *ctx = file2ctx(file);
2486         struct bcm2835_codec_q_data *q_data = &ctx->q_data[V4L2_M2M_SRC];
2487         int ret;
2488
2489         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s, cmd %u", __func__,
2490                  cmd->cmd);
2491         ret = vidioc_try_encoder_cmd(file, priv, cmd);
2492         if (ret)
2493                 return ret;
2494
2495         switch (cmd->cmd) {
2496         case V4L2_ENC_CMD_STOP:
2497                 if (q_data->eos_buffer_in_use)
2498                         v4l2_err(&ctx->dev->v4l2_dev, "EOS buffers already in use\n");
2499                 q_data->eos_buffer_in_use = true;
2500
2501                 q_data->eos_buffer.mmal.buffer_size = 0;
2502                 q_data->eos_buffer.mmal.length = 0;
2503                 q_data->eos_buffer.mmal.mmal_flags =
2504                                                 MMAL_BUFFER_HEADER_FLAG_EOS;
2505                 q_data->eos_buffer.mmal.pts = 0;
2506                 q_data->eos_buffer.mmal.dts = 0;
2507
2508                 if (!ctx->component)
2509                         break;
2510
2511                 ret = vchiq_mmal_submit_buffer(ctx->dev->instance,
2512                                                &ctx->component->input[0],
2513                                                &q_data->eos_buffer.mmal);
2514                 if (ret)
2515                         v4l2_err(&ctx->dev->v4l2_dev,
2516                                  "%s: EOS buffer submit failed %d\n",
2517                                  __func__, ret);
2518
2519                 break;
2520         case V4L2_ENC_CMD_START:
2521                 /* Do we need to do anything here? */
2522                 break;
2523
2524         default:
2525                 return -EINVAL;
2526         }
2527
2528         return 0;
2529 }
2530
2531 static int vidioc_enum_framesizes(struct file *file, void *fh,
2532                                   struct v4l2_frmsizeenum *fsize)
2533 {
2534         struct bcm2835_codec_fmt *fmt;
2535
2536         fmt = find_format_pix_fmt(fsize->pixel_format, file2ctx(file)->dev,
2537                                   true);
2538         if (!fmt)
2539                 fmt = find_format_pix_fmt(fsize->pixel_format,
2540                                           file2ctx(file)->dev,
2541                                           false);
2542
2543         if (!fmt)
2544                 return -EINVAL;
2545
2546         if (fsize->index)
2547                 return -EINVAL;
2548
2549         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
2550
2551         fsize->stepwise.min_width = MIN_W;
2552         fsize->stepwise.max_width = MAX_W;
2553         fsize->stepwise.step_width = 2;
2554         fsize->stepwise.min_height = MIN_H;
2555         fsize->stepwise.max_height = MAX_H;
2556         fsize->stepwise.step_height = 2;
2557
2558         return 0;
2559 }
2560
2561 static const struct v4l2_ioctl_ops bcm2835_codec_ioctl_ops = {
2562         .vidioc_querycap        = vidioc_querycap,
2563
2564         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
2565         .vidioc_g_fmt_vid_cap_mplane    = vidioc_g_fmt_vid_cap,
2566         .vidioc_try_fmt_vid_cap_mplane  = vidioc_try_fmt_vid_cap,
2567         .vidioc_s_fmt_vid_cap_mplane    = vidioc_s_fmt_vid_cap,
2568
2569         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
2570         .vidioc_g_fmt_vid_out_mplane    = vidioc_g_fmt_vid_out,
2571         .vidioc_try_fmt_vid_out_mplane  = vidioc_try_fmt_vid_out,
2572         .vidioc_s_fmt_vid_out_mplane    = vidioc_s_fmt_vid_out,
2573
2574         .vidioc_reqbufs         = v4l2_m2m_ioctl_reqbufs,
2575         .vidioc_querybuf        = v4l2_m2m_ioctl_querybuf,
2576         .vidioc_qbuf            = v4l2_m2m_ioctl_qbuf,
2577         .vidioc_dqbuf           = v4l2_m2m_ioctl_dqbuf,
2578         .vidioc_prepare_buf     = v4l2_m2m_ioctl_prepare_buf,
2579         .vidioc_create_bufs     = v4l2_m2m_ioctl_create_bufs,
2580         .vidioc_expbuf          = v4l2_m2m_ioctl_expbuf,
2581
2582         .vidioc_streamon        = v4l2_m2m_ioctl_streamon,
2583         .vidioc_streamoff       = v4l2_m2m_ioctl_streamoff,
2584
2585         .vidioc_g_selection     = vidioc_g_selection,
2586         .vidioc_s_selection     = vidioc_s_selection,
2587
2588         .vidioc_g_parm          = vidioc_g_parm,
2589         .vidioc_s_parm          = vidioc_s_parm,
2590
2591         .vidioc_g_pixelaspect   = vidioc_g_pixelaspect,
2592
2593         .vidioc_subscribe_event = vidioc_subscribe_evt,
2594         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
2595
2596         .vidioc_decoder_cmd = vidioc_decoder_cmd,
2597         .vidioc_try_decoder_cmd = vidioc_try_decoder_cmd,
2598         .vidioc_encoder_cmd = vidioc_encoder_cmd,
2599         .vidioc_try_encoder_cmd = vidioc_try_encoder_cmd,
2600         .vidioc_enum_framesizes = vidioc_enum_framesizes,
2601 };
2602
2603 static int bcm2835_codec_create_component(struct bcm2835_codec_ctx *ctx)
2604 {
2605         struct bcm2835_codec_dev *dev = ctx->dev;
2606         unsigned int enable = 1;
2607         int ret;
2608
2609         ret = vchiq_mmal_component_init(dev->instance, components[dev->role],
2610                                         &ctx->component);
2611         if (ret < 0) {
2612                 v4l2_err(&dev->v4l2_dev, "%s: failed to create component %s\n",
2613                          __func__, components[dev->role]);
2614                 return -ENOMEM;
2615         }
2616
2617         vchiq_mmal_port_parameter_set(dev->instance, &ctx->component->input[0],
2618                                       MMAL_PARAMETER_ZERO_COPY, &enable,
2619                                       sizeof(enable));
2620         vchiq_mmal_port_parameter_set(dev->instance, &ctx->component->output[0],
2621                                       MMAL_PARAMETER_ZERO_COPY, &enable,
2622                                       sizeof(enable));
2623
2624         if (dev->role == DECODE) {
2625                 /*
2626                  * Disable firmware option that ensures decoded timestamps
2627                  * always increase.
2628                  */
2629                 enable = 0;
2630                 vchiq_mmal_port_parameter_set(dev->instance,
2631                                               &ctx->component->output[0],
2632                                               MMAL_PARAMETER_VIDEO_VALIDATE_TIMESTAMPS,
2633                                               &enable,
2634                                               sizeof(enable));
2635                 /*
2636                  * Enable firmware option to stop on colourspace and pixel
2637                  * aspect ratio changed
2638                  */
2639                 enable = 1;
2640                 vchiq_mmal_port_parameter_set(dev->instance,
2641                                               &ctx->component->control,
2642                                               MMAL_PARAMETER_VIDEO_STOP_ON_PAR_COLOUR_CHANGE,
2643                                               &enable,
2644                                               sizeof(enable));
2645         } else if (dev->role == DEINTERLACE) {
2646                 /* Select the default deinterlace algorithm. */
2647                 int half_framerate = 0;
2648                 int default_frame_interval = -1; /* don't interpolate */
2649                 int frame_type = 5; /* 0=progressive, 3=TFF, 4=BFF, 5=see frame */
2650                 int use_qpus = 0;
2651                 enum mmal_parameter_imagefx effect =
2652                         advanced_deinterlace && ctx->q_data[V4L2_M2M_SRC].crop_width <= 800 ?
2653                         MMAL_PARAM_IMAGEFX_DEINTERLACE_ADV :
2654                         MMAL_PARAM_IMAGEFX_DEINTERLACE_FAST;
2655                 struct mmal_parameter_imagefx_parameters params = {
2656                         .effect = effect,
2657                         .num_effect_params = 4,
2658                         .effect_parameter = { frame_type,
2659                                               default_frame_interval,
2660                                               half_framerate,
2661                                               use_qpus },
2662                 };
2663
2664                 vchiq_mmal_port_parameter_set(dev->instance,
2665                                               &ctx->component->output[0],
2666                                               MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,
2667                                               &params,
2668                                               sizeof(params));
2669
2670         } else if (dev->role == ENCODE_IMAGE) {
2671                 enable = 0;
2672                 vchiq_mmal_port_parameter_set(dev->instance,
2673                                               &ctx->component->control,
2674                                               MMAL_PARAMETER_EXIF_DISABLE,
2675                                               &enable,
2676                                               sizeof(enable));
2677                 enable = 1;
2678                 vchiq_mmal_port_parameter_set(dev->instance,
2679                                               &ctx->component->output[0],
2680                                                   MMAL_PARAMETER_JPEG_IJG_SCALING,
2681                                               &enable,
2682                                               sizeof(enable));
2683         }
2684
2685         setup_mmal_port_format(ctx, &ctx->q_data[V4L2_M2M_SRC],
2686                                &ctx->component->input[0]);
2687         ctx->component->input[0].cb_ctx = ctx;
2688
2689         setup_mmal_port_format(ctx, &ctx->q_data[V4L2_M2M_DST],
2690                                &ctx->component->output[0]);
2691         ctx->component->output[0].cb_ctx = ctx;
2692
2693         ret = vchiq_mmal_port_set_format(dev->instance,
2694                                          &ctx->component->input[0]);
2695         if (ret < 0) {
2696                 v4l2_dbg(1, debug, &dev->v4l2_dev,
2697                          "%s: vchiq_mmal_port_set_format ip port failed\n",
2698                          __func__);
2699                 goto destroy_component;
2700         }
2701
2702         ret = vchiq_mmal_port_set_format(dev->instance,
2703                                          &ctx->component->output[0]);
2704         if (ret < 0) {
2705                 v4l2_dbg(1, debug, &dev->v4l2_dev,
2706                          "%s: vchiq_mmal_port_set_format op port failed\n",
2707                          __func__);
2708                 goto destroy_component;
2709         }
2710
2711         if (dev->role == ENCODE || dev->role == ENCODE_IMAGE) {
2712                 u32 param = 1;
2713
2714                 if (ctx->q_data[V4L2_M2M_SRC].sizeimage <
2715                         ctx->component->output[0].minimum_buffer.size)
2716                         v4l2_err(&dev->v4l2_dev, "buffer size mismatch sizeimage %u < min size %u\n",
2717                                  ctx->q_data[V4L2_M2M_SRC].sizeimage,
2718                                  ctx->component->output[0].minimum_buffer.size);
2719
2720                 if (dev->role == ENCODE) {
2721                         /* Enable SPS Timing header so framerate information is encoded
2722                          * in the H264 header.
2723                          */
2724                         vchiq_mmal_port_parameter_set(ctx->dev->instance,
2725                                                       &ctx->component->output[0],
2726                                                       MMAL_PARAMETER_VIDEO_ENCODE_SPS_TIMING,
2727                                                       &param, sizeof(param));
2728
2729                         /* Enable inserting headers into the first frame */
2730                         vchiq_mmal_port_parameter_set(ctx->dev->instance,
2731                                                       &ctx->component->control,
2732                                                       MMAL_PARAMETER_VIDEO_ENCODE_HEADERS_WITH_FRAME,
2733                                                       &param, sizeof(param));
2734                         /*
2735                          * Avoid fragmenting the buffers over multiple frames (unless
2736                          * the frame is bigger than the whole buffer)
2737                          */
2738                         vchiq_mmal_port_parameter_set(ctx->dev->instance,
2739                                                       &ctx->component->control,
2740                                                       MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
2741                                                       &param, sizeof(param));
2742                 }
2743         } else {
2744                 if (ctx->q_data[V4L2_M2M_DST].sizeimage <
2745                         ctx->component->output[0].minimum_buffer.size)
2746                         v4l2_err(&dev->v4l2_dev, "buffer size mismatch sizeimage %u < min size %u\n",
2747                                  ctx->q_data[V4L2_M2M_DST].sizeimage,
2748                                  ctx->component->output[0].minimum_buffer.size);
2749         }
2750
2751         /* Now we have a component we can set all the ctrls */
2752         ret = v4l2_ctrl_handler_setup(&ctx->hdl);
2753
2754         v4l2_dbg(2, debug, &dev->v4l2_dev, "%s: component created as %s\n",
2755                  __func__, components[dev->role]);
2756
2757         return 0;
2758
2759 destroy_component:
2760         vchiq_mmal_component_finalise(ctx->dev->instance, ctx->component);
2761         ctx->component = NULL;
2762
2763         return ret;
2764 }
2765
2766 /*
2767  * Queue operations
2768  */
2769
2770 static int bcm2835_codec_queue_setup(struct vb2_queue *vq,
2771                                      unsigned int *nbuffers,
2772                                      unsigned int *nplanes,
2773                                      unsigned int sizes[],
2774                                      struct device *alloc_devs[])
2775 {
2776         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vq);
2777         struct bcm2835_codec_q_data *q_data;
2778         struct vchiq_mmal_port *port;
2779         unsigned int size;
2780
2781         q_data = get_q_data(ctx, vq->type);
2782         if (!q_data)
2783                 return -EINVAL;
2784
2785         if (!ctx->component)
2786                 if (bcm2835_codec_create_component(ctx))
2787                         return -EINVAL;
2788
2789         port = get_port_data(ctx, vq->type);
2790
2791         size = q_data->sizeimage;
2792
2793         if (*nplanes)
2794                 return sizes[0] < size ? -EINVAL : 0;
2795
2796         *nplanes = 1;
2797
2798         sizes[0] = size;
2799         port->current_buffer.size = size;
2800
2801         if (*nbuffers < port->minimum_buffer.num)
2802                 *nbuffers = port->minimum_buffer.num;
2803         /* Add one buffer to take an EOS */
2804         port->current_buffer.num = *nbuffers + 1;
2805
2806         return 0;
2807 }
2808
2809 static int bcm2835_codec_mmal_buf_cleanup(struct mmal_buffer *mmal_buf)
2810 {
2811         mmal_vchi_buffer_cleanup(mmal_buf);
2812
2813         if (mmal_buf->dma_buf) {
2814                 dma_buf_put(mmal_buf->dma_buf);
2815                 mmal_buf->dma_buf = NULL;
2816         }
2817
2818         return 0;
2819 }
2820
2821 static int bcm2835_codec_buf_init(struct vb2_buffer *vb)
2822 {
2823         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
2824         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
2825         struct v4l2_m2m_buffer *m2m = container_of(vb2, struct v4l2_m2m_buffer,
2826                                                    vb);
2827         struct m2m_mmal_buffer *buf = container_of(m2m, struct m2m_mmal_buffer,
2828                                                    m2m);
2829
2830         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: ctx:%p, vb %p\n",
2831                  __func__, ctx, vb);
2832         buf->mmal.buffer = vb2_plane_vaddr(&buf->m2m.vb.vb2_buf, 0);
2833         buf->mmal.buffer_size = vb2_plane_size(&buf->m2m.vb.vb2_buf, 0);
2834
2835         mmal_vchi_buffer_init(ctx->dev->instance, &buf->mmal);
2836
2837         return 0;
2838 }
2839
2840 static int bcm2835_codec_buf_prepare(struct vb2_buffer *vb)
2841 {
2842         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
2843         struct bcm2835_codec_q_data *q_data;
2844         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
2845         struct v4l2_m2m_buffer *m2m = container_of(vbuf, struct v4l2_m2m_buffer,
2846                                                    vb);
2847         struct m2m_mmal_buffer *buf = container_of(m2m, struct m2m_mmal_buffer,
2848                                                    m2m);
2849         struct dma_buf *dma_buf;
2850         int ret;
2851
2852         v4l2_dbg(4, debug, &ctx->dev->v4l2_dev, "%s: type: %d ptr %p\n",
2853                  __func__, vb->vb2_queue->type, vb);
2854
2855         q_data = get_q_data(ctx, vb->vb2_queue->type);
2856         if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
2857                 if (vbuf->field == V4L2_FIELD_ANY)
2858                         vbuf->field = V4L2_FIELD_NONE;
2859         }
2860
2861         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
2862                 v4l2_err(&ctx->dev->v4l2_dev, "%s data will not fit into plane (%lu < %lu)\n",
2863                          __func__, vb2_plane_size(vb, 0),
2864                          (long)q_data->sizeimage);
2865                 return -EINVAL;
2866         }
2867
2868         if (!V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type))
2869                 vb2_set_plane_payload(vb, 0, q_data->sizeimage);
2870
2871         switch (vb->memory) {
2872         case VB2_MEMORY_DMABUF:
2873                 dma_buf = dma_buf_get(vb->planes[0].m.fd);
2874
2875                 if (dma_buf != buf->mmal.dma_buf) {
2876                         /* dmabuf either hasn't already been mapped, or it has
2877                          * changed.
2878                          */
2879                         if (buf->mmal.dma_buf) {
2880                                 v4l2_err(&ctx->dev->v4l2_dev,
2881                                          "%s Buffer changed - why did the core not call cleanup?\n",
2882                                          __func__);
2883                                 bcm2835_codec_mmal_buf_cleanup(&buf->mmal);
2884                         }
2885
2886                         buf->mmal.dma_buf = dma_buf;
2887                 } else {
2888                         /* We already have a reference count on the dmabuf, so
2889                          * release the one we acquired above.
2890                          */
2891                         dma_buf_put(dma_buf);
2892                 }
2893                 ret = 0;
2894                 break;
2895         case VB2_MEMORY_MMAP:
2896                 /*
2897                  * We want to do this at init, but vb2_core_expbuf checks that
2898                  * the index < q->num_buffers, and q->num_buffers only gets
2899                  * updated once all the buffers are allocated.
2900                  */
2901                 if (!buf->mmal.dma_buf) {
2902                         ret = vb2_core_expbuf_dmabuf(vb->vb2_queue,
2903                                                      vb->vb2_queue->type,
2904                                                      vb->index, 0,
2905                                                      O_CLOEXEC,
2906                                                      &buf->mmal.dma_buf);
2907                         if (ret)
2908                                 v4l2_err(&ctx->dev->v4l2_dev,
2909                                          "%s: Failed to expbuf idx %d, ret %d\n",
2910                                          __func__, vb->index, ret);
2911                 } else {
2912                         ret = 0;
2913                 }
2914                 break;
2915         default:
2916                 ret = -EINVAL;
2917                 break;
2918         }
2919
2920         return ret;
2921 }
2922
2923 static void bcm2835_codec_buf_queue(struct vb2_buffer *vb)
2924 {
2925         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
2926         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
2927
2928         v4l2_dbg(4, debug, &ctx->dev->v4l2_dev, "%s: type: %d ptr %p vbuf->flags %u, seq %u, bytesused %u\n",
2929                  __func__, vb->vb2_queue->type, vb, vbuf->flags, vbuf->sequence,
2930                  vb->planes[0].bytesused);
2931         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
2932 }
2933
2934 static void bcm2835_codec_buffer_cleanup(struct vb2_buffer *vb)
2935 {
2936         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
2937         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
2938         struct v4l2_m2m_buffer *m2m = container_of(vb2, struct v4l2_m2m_buffer,
2939                                                    vb);
2940         struct m2m_mmal_buffer *buf = container_of(m2m, struct m2m_mmal_buffer,
2941                                                    m2m);
2942
2943         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: ctx:%p, vb %p\n",
2944                  __func__, ctx, vb);
2945
2946         bcm2835_codec_mmal_buf_cleanup(&buf->mmal);
2947 }
2948
2949 static void bcm2835_codec_flush_buffers(struct bcm2835_codec_ctx *ctx,
2950                                         struct vchiq_mmal_port *port)
2951 {
2952         int ret;
2953
2954         if (atomic_read(&port->buffers_with_vpu)) {
2955                 v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Waiting for buffers to be returned - %d outstanding\n",
2956                          __func__, atomic_read(&port->buffers_with_vpu));
2957                 ret = wait_for_completion_timeout(&ctx->frame_cmplt,
2958                                                   COMPLETE_TIMEOUT);
2959                 if (ret <= 0) {
2960                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
2961                                  __func__,
2962                                  atomic_read(&port->buffers_with_vpu));
2963                 }
2964         }
2965 }
2966 static int bcm2835_codec_start_streaming(struct vb2_queue *q,
2967                                          unsigned int count)
2968 {
2969         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(q);
2970         struct bcm2835_codec_dev *dev = ctx->dev;
2971         struct bcm2835_codec_q_data *q_data = get_q_data(ctx, q->type);
2972         struct vchiq_mmal_port *port = get_port_data(ctx, q->type);
2973         int ret = 0;
2974
2975         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: type: %d count %d\n",
2976                  __func__, q->type, count);
2977         q_data->sequence = 0;
2978
2979         if (!ctx->component_enabled) {
2980                 ret = vchiq_mmal_component_enable(dev->instance,
2981                                                   ctx->component);
2982                 if (ret)
2983                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling component, ret %d\n",
2984                                  __func__, ret);
2985                 ctx->component_enabled = true;
2986         }
2987
2988         if (port->enabled) {
2989                 unsigned int num_buffers;
2990
2991                 init_completion(&ctx->frame_cmplt);
2992
2993                 /*
2994                  * This should only ever happen with DECODE and the MMAL output
2995                  * port that has been enabled for resolution changed events.
2996                  * In this case no buffers have been allocated or sent to the
2997                  * component, so warn on that.
2998                  */
2999                 WARN_ON(ctx->dev->role != DECODE);
3000                 WARN_ON(q->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
3001                 WARN_ON(atomic_read(&port->buffers_with_vpu));
3002
3003                 /*
3004                  * Disable will reread the port format, so retain buffer count.
3005                  */
3006                 num_buffers = port->current_buffer.num;
3007
3008                 ret = vchiq_mmal_port_disable(dev->instance, port);
3009                 if (ret)
3010                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Error disabling port update buffer count, ret %d\n",
3011                                  __func__, ret);
3012                 bcm2835_codec_flush_buffers(ctx, port);
3013                 port->current_buffer.num = num_buffers;
3014         }
3015
3016         if (count < port->minimum_buffer.num)
3017                 count = port->minimum_buffer.num;
3018
3019         if (port->current_buffer.num < count + 1) {
3020                 v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: ctx:%p, buffer count changed %u to %u\n",
3021                          __func__, ctx, port->current_buffer.num, count + 1);
3022
3023                 port->current_buffer.num = count + 1;
3024                 ret = vchiq_mmal_port_set_format(dev->instance, port);
3025                 if (ret)
3026                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Error updating buffer count, ret %d\n",
3027                                  __func__, ret);
3028         }
3029
3030         if (dev->role == DECODE &&
3031             q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
3032             !ctx->component->output[0].enabled) {
3033                 /*
3034                  * Decode needs to enable the MMAL output/V4L2 CAPTURE
3035                  * port at this point too so that we have everything
3036                  * set up for dynamic resolution changes.
3037                  */
3038                 ret = vchiq_mmal_port_enable(dev->instance,
3039                                              &ctx->component->output[0],
3040                                              op_buffer_cb);
3041                 if (ret)
3042                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling o/p port, ret %d\n",
3043                                  __func__, ret);
3044         }
3045
3046         if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
3047                 /*
3048                  * Create the EOS buffer.
3049                  * We only need the MMAL part, and want to NOT attach a memory
3050                  * buffer to it as it should only take flags.
3051                  */
3052                 memset(&q_data->eos_buffer, 0, sizeof(q_data->eos_buffer));
3053                 mmal_vchi_buffer_init(dev->instance,
3054                                       &q_data->eos_buffer.mmal);
3055                 q_data->eos_buffer_in_use = false;
3056
3057                 ret = vchiq_mmal_port_enable(dev->instance,
3058                                              port,
3059                                              ip_buffer_cb);
3060                 if (ret)
3061                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling i/p port, ret %d\n",
3062                                  __func__, ret);
3063         } else {
3064                 if (!port->enabled) {
3065                         ret = vchiq_mmal_port_enable(dev->instance,
3066                                                      port,
3067                                                      op_buffer_cb);
3068                         if (ret)
3069                                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling o/p port, ret %d\n",
3070                                          __func__, ret);
3071                 }
3072         }
3073         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Done, ret %d\n",
3074                  __func__, ret);
3075         return ret;
3076 }
3077
3078 static void bcm2835_codec_stop_streaming(struct vb2_queue *q)
3079 {
3080         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(q);
3081         struct bcm2835_codec_dev *dev = ctx->dev;
3082         struct bcm2835_codec_q_data *q_data = get_q_data(ctx, q->type);
3083         struct vchiq_mmal_port *port = get_port_data(ctx, q->type);
3084         struct vb2_v4l2_buffer *vbuf;
3085         int ret;
3086
3087         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: type: %d - return buffers\n",
3088                  __func__, q->type);
3089
3090         init_completion(&ctx->frame_cmplt);
3091
3092         /* Clear out all buffers held by m2m framework */
3093         for (;;) {
3094                 if (V4L2_TYPE_IS_OUTPUT(q->type))
3095                         vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
3096                 else
3097                         vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
3098                 if (!vbuf)
3099                         break;
3100                 v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: return buffer %p\n",
3101                          __func__, vbuf);
3102
3103                 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_QUEUED);
3104         }
3105
3106         /* Disable MMAL port - this will flush buffers back */
3107         ret = vchiq_mmal_port_disable(dev->instance, port);
3108         if (ret)
3109                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed disabling %s port, ret %d\n",
3110                          __func__, V4L2_TYPE_IS_OUTPUT(q->type) ? "i/p" : "o/p",
3111                          ret);
3112
3113         bcm2835_codec_flush_buffers(ctx, port);
3114
3115         if (dev->role == DECODE &&
3116             q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
3117             ctx->component->input[0].enabled) {
3118                 /*
3119                  * For decode we need to keep the MMAL output port enabled for
3120                  * resolution changed events whenever the input is enabled.
3121                  */
3122                 ret = vchiq_mmal_port_enable(dev->instance,
3123                                              &ctx->component->output[0],
3124                                              op_buffer_cb);
3125                 if (ret)
3126                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling o/p port, ret %d\n",
3127                                  __func__, ret);
3128         }
3129
3130         /* If both ports disabled, then disable the component */
3131         if (ctx->component_enabled &&
3132             !ctx->component->input[0].enabled &&
3133             !ctx->component->output[0].enabled) {
3134                 ret = vchiq_mmal_component_disable(dev->instance,
3135                                                    ctx->component);
3136                 if (ret)
3137                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling component, ret %d\n",
3138                                  __func__, ret);
3139                 ctx->component_enabled = false;
3140         }
3141
3142         if (V4L2_TYPE_IS_OUTPUT(q->type))
3143                 mmal_vchi_buffer_cleanup(&q_data->eos_buffer.mmal);
3144
3145         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: done\n", __func__);
3146 }
3147
3148 static const struct vb2_ops bcm2835_codec_qops = {
3149         .queue_setup     = bcm2835_codec_queue_setup,
3150         .buf_init        = bcm2835_codec_buf_init,
3151         .buf_prepare     = bcm2835_codec_buf_prepare,
3152         .buf_queue       = bcm2835_codec_buf_queue,
3153         .buf_cleanup     = bcm2835_codec_buffer_cleanup,
3154         .start_streaming = bcm2835_codec_start_streaming,
3155         .stop_streaming  = bcm2835_codec_stop_streaming,
3156         .wait_prepare    = vb2_ops_wait_prepare,
3157         .wait_finish     = vb2_ops_wait_finish,
3158 };
3159
3160 static int queue_init(void *priv, struct vb2_queue *src_vq,
3161                       struct vb2_queue *dst_vq)
3162 {
3163         struct bcm2835_codec_ctx *ctx = priv;
3164         int ret;
3165
3166         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
3167         src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
3168         src_vq->drv_priv = ctx;
3169         src_vq->buf_struct_size = sizeof(struct m2m_mmal_buffer);
3170         src_vq->ops = &bcm2835_codec_qops;
3171         src_vq->mem_ops = &vb2_dma_contig_memops;
3172         src_vq->dev = &ctx->dev->pdev->dev;
3173         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
3174         src_vq->lock = &ctx->dev->dev_mutex;
3175
3176         ret = vb2_queue_init(src_vq);
3177         if (ret)
3178                 return ret;
3179
3180         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
3181         dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
3182         dst_vq->drv_priv = ctx;
3183         dst_vq->buf_struct_size = sizeof(struct m2m_mmal_buffer);
3184         dst_vq->ops = &bcm2835_codec_qops;
3185         dst_vq->mem_ops = &vb2_dma_contig_memops;
3186         dst_vq->dev = &ctx->dev->pdev->dev;
3187         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
3188         dst_vq->lock = &ctx->dev->dev_mutex;
3189
3190         return vb2_queue_init(dst_vq);
3191 }
3192
3193 /*
3194  * File operations
3195  */
3196 static int bcm2835_codec_open(struct file *file)
3197 {
3198         struct bcm2835_codec_dev *dev = video_drvdata(file);
3199         struct bcm2835_codec_ctx *ctx = NULL;
3200         struct v4l2_ctrl_handler *hdl;
3201         int rc = 0;
3202
3203         if (mutex_lock_interruptible(&dev->dev_mutex)) {
3204                 v4l2_err(&dev->v4l2_dev, "Mutex fail\n");
3205                 return -ERESTARTSYS;
3206         }
3207         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
3208         if (!ctx) {
3209                 rc = -ENOMEM;
3210                 goto open_unlock;
3211         }
3212
3213         ctx->q_data[V4L2_M2M_SRC].fmt = get_default_format(dev, false);
3214         ctx->q_data[V4L2_M2M_DST].fmt = get_default_format(dev, true);
3215
3216         ctx->q_data[V4L2_M2M_SRC].crop_width = DEFAULT_WIDTH;
3217         ctx->q_data[V4L2_M2M_SRC].crop_height = DEFAULT_HEIGHT;
3218         ctx->q_data[V4L2_M2M_SRC].height = DEFAULT_HEIGHT;
3219         ctx->q_data[V4L2_M2M_SRC].bytesperline =
3220                         get_bytesperline(DEFAULT_WIDTH, DEFAULT_HEIGHT,
3221                                          ctx->q_data[V4L2_M2M_SRC].fmt,
3222                                          dev->role);
3223         ctx->q_data[V4L2_M2M_SRC].sizeimage =
3224                 get_sizeimage(ctx->q_data[V4L2_M2M_SRC].bytesperline,
3225                               ctx->q_data[V4L2_M2M_SRC].crop_width,
3226                               ctx->q_data[V4L2_M2M_SRC].height,
3227                               ctx->q_data[V4L2_M2M_SRC].fmt);
3228         ctx->q_data[V4L2_M2M_SRC].field = V4L2_FIELD_NONE;
3229
3230         ctx->q_data[V4L2_M2M_DST].crop_width = DEFAULT_WIDTH;
3231         ctx->q_data[V4L2_M2M_DST].crop_height = DEFAULT_HEIGHT;
3232         ctx->q_data[V4L2_M2M_DST].height = DEFAULT_HEIGHT;
3233         ctx->q_data[V4L2_M2M_DST].bytesperline =
3234                         get_bytesperline(DEFAULT_WIDTH, DEFAULT_HEIGHT,
3235                                          ctx->q_data[V4L2_M2M_DST].fmt,
3236                                          dev->role);
3237         ctx->q_data[V4L2_M2M_DST].sizeimage =
3238                 get_sizeimage(ctx->q_data[V4L2_M2M_DST].bytesperline,
3239                               ctx->q_data[V4L2_M2M_DST].crop_width,
3240                               ctx->q_data[V4L2_M2M_DST].height,
3241                               ctx->q_data[V4L2_M2M_DST].fmt);
3242         ctx->q_data[V4L2_M2M_DST].aspect_ratio.numerator = 1;
3243         ctx->q_data[V4L2_M2M_DST].aspect_ratio.denominator = 1;
3244         ctx->q_data[V4L2_M2M_DST].field = V4L2_FIELD_NONE;
3245
3246         ctx->colorspace = V4L2_COLORSPACE_REC709;
3247         ctx->bitrate = 10 * 1000 * 1000;
3248
3249         ctx->framerate_num = 30;
3250         ctx->framerate_denom = 1;
3251
3252         /* Initialise V4L2 contexts */
3253         v4l2_fh_init(&ctx->fh, video_devdata(file));
3254         file->private_data = &ctx->fh;
3255         ctx->dev = dev;
3256         hdl = &ctx->hdl;
3257         switch (dev->role) {
3258         case ENCODE:
3259         {
3260                 /* Encode controls */
3261                 v4l2_ctrl_handler_init(hdl, 11);
3262
3263                 v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
3264                                        V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
3265                                        V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 0,
3266                                        V4L2_MPEG_VIDEO_BITRATE_MODE_VBR);
3267                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3268                                   V4L2_CID_MPEG_VIDEO_BITRATE,
3269                                   25 * 1000, 25 * 1000 * 1000,
3270                                   25 * 1000, 10 * 1000 * 1000);
3271                 v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
3272                                        V4L2_CID_MPEG_VIDEO_HEADER_MODE,
3273                                        V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
3274                                        0, V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME);
3275                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3276                                   V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER,
3277                                   0, 1,
3278                                   1, 0);
3279                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3280                                   V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
3281                                   0, 0x7FFFFFFF,
3282                                   1, 60);
3283                 v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
3284                                        V4L2_CID_MPEG_VIDEO_H264_LEVEL,
3285                                        V4L2_MPEG_VIDEO_H264_LEVEL_5_1,
3286                                        ~(BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_0) |
3287                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1B) |
3288                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_1) |
3289                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_2) |
3290                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_3) |
3291                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_0) |
3292                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_1) |
3293                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_2) |
3294                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_0) |
3295                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_1) |
3296                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_2) |
3297                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_0) |
3298                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_1) |
3299                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_2) |
3300                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_5_0) |
3301                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_5_1)),
3302                                        V4L2_MPEG_VIDEO_H264_LEVEL_4_0);
3303                 v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
3304                                        V4L2_CID_MPEG_VIDEO_H264_PROFILE,
3305                                        V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
3306                                        ~(BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
3307                                          BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
3308                                          BIT(V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
3309                                          BIT(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)),
3310                                         V4L2_MPEG_VIDEO_H264_PROFILE_HIGH);
3311                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3312                                   V4L2_CID_MPEG_VIDEO_H264_MIN_QP,
3313                                   0, 51,
3314                                   1, 20);
3315                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3316                                   V4L2_CID_MPEG_VIDEO_H264_MAX_QP,
3317                                   0, 51,
3318                                   1, 51);
3319                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3320                                   V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME,
3321                                   0, 0, 0, 0);
3322                 if (hdl->error) {
3323                         rc = hdl->error;
3324                         goto free_ctrl_handler;
3325                 }
3326                 ctx->fh.ctrl_handler = hdl;
3327                 v4l2_ctrl_handler_setup(hdl);
3328         }
3329         break;
3330         case DECODE:
3331         {
3332                 v4l2_ctrl_handler_init(hdl, 1);
3333
3334                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3335                                   V4L2_CID_MIN_BUFFERS_FOR_CAPTURE,
3336                                   1, 1, 1, 1);
3337                 if (hdl->error) {
3338                         rc = hdl->error;
3339                         goto free_ctrl_handler;
3340                 }
3341                 ctx->fh.ctrl_handler = hdl;
3342                 v4l2_ctrl_handler_setup(hdl);
3343         }
3344         break;
3345         case ISP:
3346         {
3347                 v4l2_ctrl_handler_init(hdl, 2);
3348
3349                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3350                                   V4L2_CID_HFLIP,
3351                                   1, 0, 1, 0);
3352                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3353                                   V4L2_CID_VFLIP,
3354                                   1, 0, 1, 0);
3355                 if (hdl->error) {
3356                         rc = hdl->error;
3357                         goto free_ctrl_handler;
3358                 }
3359                 ctx->fh.ctrl_handler = hdl;
3360                 v4l2_ctrl_handler_setup(hdl);
3361         }
3362         break;
3363         case DEINTERLACE:
3364         {
3365                 v4l2_ctrl_handler_init(hdl, 0);
3366         }
3367         break;
3368         case ENCODE_IMAGE:
3369         {
3370                 /* Encode image controls */
3371                 v4l2_ctrl_handler_init(hdl, 1);
3372
3373                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
3374                                   V4L2_CID_JPEG_COMPRESSION_QUALITY,
3375                                   1, 100,
3376                                   1, 80);
3377                 if (hdl->error) {
3378                         rc = hdl->error;
3379                         goto free_ctrl_handler;
3380                 }
3381                 ctx->fh.ctrl_handler = hdl;
3382                 v4l2_ctrl_handler_setup(hdl);
3383         }
3384         break;
3385         case NUM_ROLES:
3386         break;
3387         }
3388
3389         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
3390
3391         if (IS_ERR(ctx->fh.m2m_ctx)) {
3392                 rc = PTR_ERR(ctx->fh.m2m_ctx);
3393
3394                 goto free_ctrl_handler;
3395         }
3396
3397         /* Set both queues as buffered as we have buffering in the VPU. That
3398          * means that we will be scheduled whenever either an input or output
3399          * buffer is available (otherwise one of each are required).
3400          */
3401         v4l2_m2m_set_src_buffered(ctx->fh.m2m_ctx, true);
3402         v4l2_m2m_set_dst_buffered(ctx->fh.m2m_ctx, true);
3403
3404         v4l2_fh_add(&ctx->fh);
3405         atomic_inc(&dev->num_inst);
3406
3407         mutex_unlock(&dev->dev_mutex);
3408         return 0;
3409
3410 free_ctrl_handler:
3411         v4l2_ctrl_handler_free(hdl);
3412         kfree(ctx);
3413 open_unlock:
3414         mutex_unlock(&dev->dev_mutex);
3415         return rc;
3416 }
3417
3418 static int bcm2835_codec_release(struct file *file)
3419 {
3420         struct bcm2835_codec_dev *dev = video_drvdata(file);
3421         struct bcm2835_codec_ctx *ctx = file2ctx(file);
3422
3423         v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: Releasing instance %p\n",
3424                  __func__, ctx);
3425
3426         v4l2_fh_del(&ctx->fh);
3427         v4l2_fh_exit(&ctx->fh);
3428         v4l2_ctrl_handler_free(&ctx->hdl);
3429         mutex_lock(&dev->dev_mutex);
3430         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
3431
3432         if (ctx->component)
3433                 vchiq_mmal_component_finalise(dev->instance, ctx->component);
3434
3435         mutex_unlock(&dev->dev_mutex);
3436         kfree(ctx);
3437
3438         atomic_dec(&dev->num_inst);
3439
3440         return 0;
3441 }
3442
3443 static const struct v4l2_file_operations bcm2835_codec_fops = {
3444         .owner          = THIS_MODULE,
3445         .open           = bcm2835_codec_open,
3446         .release        = bcm2835_codec_release,
3447         .poll           = v4l2_m2m_fop_poll,
3448         .unlocked_ioctl = video_ioctl2,
3449         .mmap           = v4l2_m2m_fop_mmap,
3450 };
3451
3452 static const struct video_device bcm2835_codec_videodev = {
3453         .name           = MEM2MEM_NAME,
3454         .vfl_dir        = VFL_DIR_M2M,
3455         .fops           = &bcm2835_codec_fops,
3456         .ioctl_ops      = &bcm2835_codec_ioctl_ops,
3457         .minor          = -1,
3458         .release        = video_device_release_empty,
3459 };
3460
3461 static const struct v4l2_m2m_ops m2m_ops = {
3462         .device_run     = device_run,
3463         .job_ready      = job_ready,
3464         .job_abort      = job_abort,
3465 };
3466
3467 /* Size of the array to provide to the VPU when asking for the list of supported
3468  * formats.
3469  * The ISP component currently advertises 62 input formats, so add a small
3470  * overhead on that.
3471  */
3472 #define MAX_SUPPORTED_ENCODINGS 70
3473
3474 /* Populate dev->supported_fmts with the formats supported by those ports. */
3475 static int bcm2835_codec_get_supported_fmts(struct bcm2835_codec_dev *dev)
3476 {
3477         struct bcm2835_codec_fmt *list;
3478         struct vchiq_mmal_component *component;
3479         u32 fourccs[MAX_SUPPORTED_ENCODINGS];
3480         u32 param_size = sizeof(fourccs);
3481         unsigned int i, j, num_encodings;
3482         int ret;
3483
3484         ret = vchiq_mmal_component_init(dev->instance, components[dev->role],
3485                                         &component);
3486         if (ret < 0) {
3487                 v4l2_err(&dev->v4l2_dev, "%s: failed to create component %s\n",
3488                          __func__, components[dev->role]);
3489                 return -ENOMEM;
3490         }
3491
3492         ret = vchiq_mmal_port_parameter_get(dev->instance,
3493                                             &component->input[0],
3494                                             MMAL_PARAMETER_SUPPORTED_ENCODINGS,
3495                                             &fourccs,
3496                                             &param_size);
3497
3498         if (ret) {
3499                 if (ret == MMAL_MSG_STATUS_ENOSPC) {
3500                         v4l2_err(&dev->v4l2_dev,
3501                                  "%s: port has more encodings than we provided space for. Some are dropped (%zu vs %u).\n",
3502                                  __func__, param_size / sizeof(u32),
3503                                  MAX_SUPPORTED_ENCODINGS);
3504                         num_encodings = MAX_SUPPORTED_ENCODINGS;
3505                 } else {
3506                         v4l2_err(&dev->v4l2_dev, "%s: get_param ret %u.\n",
3507                                  __func__, ret);
3508                         ret = -EINVAL;
3509                         goto destroy_component;
3510                 }
3511         } else {
3512                 num_encodings = param_size / sizeof(u32);
3513         }
3514
3515         /* Assume at this stage that all encodings will be supported in V4L2.
3516          * Any that aren't supported will waste a very small amount of memory.
3517          */
3518         list = devm_kzalloc(&dev->pdev->dev,
3519                             sizeof(struct bcm2835_codec_fmt) * num_encodings,
3520                             GFP_KERNEL);
3521         if (!list) {
3522                 ret = -ENOMEM;
3523                 goto destroy_component;
3524         }
3525         dev->supported_fmts[0].list = list;
3526
3527         for (i = 0, j = 0; i < num_encodings; i++) {
3528                 const struct bcm2835_codec_fmt *fmt = get_fmt(fourccs[i]);
3529
3530                 if (fmt) {
3531                         list[j] = *fmt;
3532                         j++;
3533                 }
3534         }
3535         dev->supported_fmts[0].num_entries = j;
3536
3537         param_size = sizeof(fourccs);
3538         ret = vchiq_mmal_port_parameter_get(dev->instance,
3539                                             &component->output[0],
3540                                             MMAL_PARAMETER_SUPPORTED_ENCODINGS,
3541                                             &fourccs,
3542                                             &param_size);
3543
3544         if (ret) {
3545                 if (ret == MMAL_MSG_STATUS_ENOSPC) {
3546                         v4l2_err(&dev->v4l2_dev,
3547                                  "%s: port has more encodings than we provided space for. Some are dropped (%zu vs %u).\n",
3548                                  __func__, param_size / sizeof(u32),
3549                                  MAX_SUPPORTED_ENCODINGS);
3550                         num_encodings = MAX_SUPPORTED_ENCODINGS;
3551                 } else {
3552                         ret = -EINVAL;
3553                         goto destroy_component;
3554                 }
3555         } else {
3556                 num_encodings = param_size / sizeof(u32);
3557         }
3558         /* Assume at this stage that all encodings will be supported in V4L2. */
3559         list = devm_kzalloc(&dev->pdev->dev,
3560                             sizeof(struct bcm2835_codec_fmt) * num_encodings,
3561                             GFP_KERNEL);
3562         if (!list) {
3563                 ret = -ENOMEM;
3564                 goto destroy_component;
3565         }
3566         dev->supported_fmts[1].list = list;
3567
3568         for (i = 0, j = 0; i < num_encodings; i++) {
3569                 const struct bcm2835_codec_fmt *fmt = get_fmt(fourccs[i]);
3570
3571                 if (fmt) {
3572                         list[j] = *fmt;
3573                         j++;
3574                 }
3575         }
3576         dev->supported_fmts[1].num_entries = j;
3577
3578         ret = 0;
3579
3580 destroy_component:
3581         vchiq_mmal_component_finalise(dev->instance, component);
3582
3583         return ret;
3584 }
3585
3586 static int bcm2835_codec_create(struct bcm2835_codec_driver *drv,
3587                                 struct bcm2835_codec_dev **new_dev,
3588                                 enum bcm2835_codec_role role)
3589 {
3590         struct platform_device *pdev = drv->pdev;
3591         struct bcm2835_codec_dev *dev;
3592         struct video_device *vfd;
3593         int function;
3594         int video_nr;
3595         int ret;
3596
3597         dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
3598         if (!dev)
3599                 return -ENOMEM;
3600
3601         dev->pdev = pdev;
3602
3603         dev->role = role;
3604
3605         ret = vchiq_mmal_init(&dev->instance);
3606         if (ret)
3607                 return ret;
3608
3609         ret = bcm2835_codec_get_supported_fmts(dev);
3610         if (ret)
3611                 goto vchiq_finalise;
3612
3613         atomic_set(&dev->num_inst, 0);
3614         mutex_init(&dev->dev_mutex);
3615
3616         /* Initialise the video device */
3617         dev->vfd = bcm2835_codec_videodev;
3618
3619         vfd = &dev->vfd;
3620         vfd->lock = &dev->dev_mutex;
3621         vfd->v4l2_dev = &dev->v4l2_dev;
3622         vfd->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
3623         vfd->v4l2_dev->mdev = &drv->mdev;
3624
3625         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
3626         if (ret)
3627                 goto vchiq_finalise;
3628
3629         switch (role) {
3630         case DECODE:
3631                 v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
3632                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
3633                 v4l2_disable_ioctl(vfd, VIDIOC_S_PARM);
3634                 v4l2_disable_ioctl(vfd, VIDIOC_G_PARM);
3635                 function = MEDIA_ENT_F_PROC_VIDEO_DECODER;
3636                 video_nr = decode_video_nr;
3637                 break;
3638         case ENCODE:
3639                 v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
3640                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
3641                 function = MEDIA_ENT_F_PROC_VIDEO_ENCODER;
3642                 video_nr = encode_video_nr;
3643                 break;
3644         case ISP:
3645                 v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
3646                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
3647                 v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
3648                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
3649                 v4l2_disable_ioctl(vfd, VIDIOC_S_PARM);
3650                 v4l2_disable_ioctl(vfd, VIDIOC_G_PARM);
3651                 function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
3652                 video_nr = isp_video_nr;
3653                 break;
3654         case DEINTERLACE:
3655                 v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
3656                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
3657                 v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
3658                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
3659                 v4l2_disable_ioctl(vfd, VIDIOC_S_PARM);
3660                 v4l2_disable_ioctl(vfd, VIDIOC_G_PARM);
3661                 function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
3662                 video_nr = deinterlace_video_nr;
3663                 break;
3664         case ENCODE_IMAGE:
3665                 v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
3666                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
3667                 function = MEDIA_ENT_F_PROC_VIDEO_ENCODER;
3668                 video_nr = encode_image_nr;
3669                 break;
3670         default:
3671                 ret = -EINVAL;
3672                 goto unreg_dev;
3673         }
3674
3675         ret = video_register_device(vfd, VFL_TYPE_VIDEO, video_nr);
3676         if (ret) {
3677                 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
3678                 goto unreg_dev;
3679         }
3680
3681         video_set_drvdata(vfd, dev);
3682         snprintf(vfd->name, sizeof(vfd->name), "%s-%s",
3683                  bcm2835_codec_videodev.name, roles[role]);
3684         v4l2_info(&dev->v4l2_dev, "Device registered as /dev/video%d\n",
3685                   vfd->num);
3686
3687         *new_dev = dev;
3688
3689         dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
3690         if (IS_ERR(dev->m2m_dev)) {
3691                 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
3692                 ret = PTR_ERR(dev->m2m_dev);
3693                 goto err_m2m;
3694         }
3695
3696         ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd, function);
3697         if (ret)
3698                 goto err_m2m;
3699
3700         v4l2_info(&dev->v4l2_dev, "Loaded V4L2 %s\n",
3701                   roles[role]);
3702         return 0;
3703
3704 err_m2m:
3705         v4l2_m2m_release(dev->m2m_dev);
3706         video_unregister_device(&dev->vfd);
3707 unreg_dev:
3708         v4l2_device_unregister(&dev->v4l2_dev);
3709 vchiq_finalise:
3710         vchiq_mmal_finalise(dev->instance);
3711         return ret;
3712 }
3713
3714 static int bcm2835_codec_destroy(struct bcm2835_codec_dev *dev)
3715 {
3716         if (!dev)
3717                 return -ENODEV;
3718
3719         v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME ", %s\n",
3720                   roles[dev->role]);
3721         v4l2_m2m_unregister_media_controller(dev->m2m_dev);
3722         v4l2_m2m_release(dev->m2m_dev);
3723         video_unregister_device(&dev->vfd);
3724         v4l2_device_unregister(&dev->v4l2_dev);
3725         vchiq_mmal_finalise(dev->instance);
3726
3727         return 0;
3728 }
3729
3730 static int bcm2835_codec_probe(struct platform_device *pdev)
3731 {
3732         struct bcm2835_codec_driver *drv;
3733         struct media_device *mdev;
3734         int ret = 0;
3735
3736         drv = devm_kzalloc(&pdev->dev, sizeof(*drv), GFP_KERNEL);
3737         if (!drv)
3738                 return -ENOMEM;
3739
3740         drv->pdev = pdev;
3741         mdev = &drv->mdev;
3742         mdev->dev = &pdev->dev;
3743
3744         strscpy(mdev->model, bcm2835_codec_videodev.name, sizeof(mdev->model));
3745         strscpy(mdev->serial, "0000", sizeof(mdev->serial));
3746         snprintf(mdev->bus_info, sizeof(mdev->bus_info), "platform:%s",
3747                  pdev->name);
3748
3749         /* This should return the vgencmd version information or such .. */
3750         mdev->hw_revision = 1;
3751         media_device_init(mdev);
3752
3753         ret = bcm2835_codec_create(drv, &drv->decode, DECODE);
3754         if (ret)
3755                 goto out;
3756
3757         ret = bcm2835_codec_create(drv, &drv->encode, ENCODE);
3758         if (ret)
3759                 goto out;
3760
3761         ret = bcm2835_codec_create(drv, &drv->isp, ISP);
3762         if (ret)
3763                 goto out;
3764
3765         ret = bcm2835_codec_create(drv, &drv->deinterlace, DEINTERLACE);
3766         if (ret)
3767                 goto out;
3768
3769         ret = bcm2835_codec_create(drv, &drv->encode_image, ENCODE_IMAGE);
3770         if (ret)
3771                 goto out;
3772
3773         /* Register the media device node */
3774         if (media_device_register(mdev) < 0)
3775                 goto out;
3776
3777         platform_set_drvdata(pdev, drv);
3778
3779         return 0;
3780
3781 out:
3782         if (drv->encode_image) {
3783                 bcm2835_codec_destroy(drv->encode_image);
3784                 drv->encode_image = NULL;
3785         }
3786         if (drv->deinterlace) {
3787                 bcm2835_codec_destroy(drv->deinterlace);
3788                 drv->deinterlace = NULL;
3789         }
3790         if (drv->isp) {
3791                 bcm2835_codec_destroy(drv->isp);
3792                 drv->isp = NULL;
3793         }
3794         if (drv->encode) {
3795                 bcm2835_codec_destroy(drv->encode);
3796                 drv->encode = NULL;
3797         }
3798         if (drv->decode) {
3799                 bcm2835_codec_destroy(drv->decode);
3800                 drv->decode = NULL;
3801         }
3802         return ret;
3803 }
3804
3805 static int bcm2835_codec_remove(struct platform_device *pdev)
3806 {
3807         struct bcm2835_codec_driver *drv = platform_get_drvdata(pdev);
3808
3809         media_device_unregister(&drv->mdev);
3810
3811         bcm2835_codec_destroy(drv->encode_image);
3812
3813         bcm2835_codec_destroy(drv->deinterlace);
3814
3815         bcm2835_codec_destroy(drv->isp);
3816
3817         bcm2835_codec_destroy(drv->encode);
3818
3819         bcm2835_codec_destroy(drv->decode);
3820
3821         media_device_cleanup(&drv->mdev);
3822
3823         return 0;
3824 }
3825
3826 static struct platform_driver bcm2835_v4l2_codec_driver = {
3827         .probe = bcm2835_codec_probe,
3828         .remove = bcm2835_codec_remove,
3829         .driver = {
3830                    .name = "bcm2835-codec",
3831                    .owner = THIS_MODULE,
3832                    },
3833 };
3834
3835 module_platform_driver(bcm2835_v4l2_codec_driver);
3836
3837 MODULE_DESCRIPTION("BCM2835 codec V4L2 driver");
3838 MODULE_AUTHOR("Dave Stevenson, <dave.stevenson@raspberrypi.com>");
3839 MODULE_LICENSE("GPL");
3840 MODULE_VERSION("0.0.1");
3841 MODULE_ALIAS("platform:bcm2835-codec");