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.org)
   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 /*
  62  * Workaround for GStreamer v4l2convert component not considering Bayer formats
  63  * as raw, and therefore not considering a V4L2 device that supports them as
  64  * as a suitable candidate.
  65  */
  66 static bool disable_bayer;
  67 module_param(disable_bayer, bool, 0644);
  68 MODULE_PARM_DESC(disable_bayer, "Disable support for Bayer formats");
  69
  70 static unsigned int debug;
  71 module_param(debug, uint, 0644);
  72 MODULE_PARM_DESC(debug, "activates debug info (0-3)");
  73
  74 enum bcm2835_codec_role {
  75         DECODE,
  76         ENCODE,
  77         ISP,
  78 };
  79
  80 static const char * const roles[] = {
  81         "decode",
  82         "encode",
  83         "isp"
  84 };
  85
  86 static const char * const components[] = {
  87         "ril.video_decode",
  88         "ril.video_encode",
  89         "ril.isp",
  90 };
  91
  92 #define MIN_W           32
  93 #define MIN_H           32
  94 #define MAX_W           1920
  95 #define MAX_H           1920
  96 #define BPL_ALIGN       32
  97 #define DEFAULT_WIDTH   640
  98 #define DEFAULT_HEIGHT  480
  99 /*
 100  * The unanswered question - what is the maximum size of a compressed frame?
 101  * V4L2 mandates that the encoded frame must fit in a single buffer. Sizing
 102  * that buffer is a compromise between wasting memory and risking not fitting.
 103  * The 1080P version of Big Buck Bunny has some frames that exceed 512kB.
 104  * Adopt a moderately arbitrary split at 720P for switching between 512 and
 105  * 768kB buffers.
 106  */
 107 #define DEF_COMP_BUF_SIZE_GREATER_720P  (768 << 10)
 108 #define DEF_COMP_BUF_SIZE_720P_OR_LESS  (512 << 10)
 109
 110 /* Flags that indicate a format can be used for capture/output */
 111 #define MEM2MEM_CAPTURE         BIT(0)
 112 #define MEM2MEM_OUTPUT          BIT(1)
 113
 114 #define MEM2MEM_NAME            "bcm2835-codec"
 115
 116 struct bcm2835_codec_fmt {
 117         u32     fourcc;
 118         int     depth;
 119         int     bytesperline_align;
 120         u32     flags;
 121         u32     mmal_fmt;
 122         int     size_multiplier_x2;
 123         bool    is_bayer;
 124 };
 125
 126 static const struct bcm2835_codec_fmt supported_formats[] = {
 127         {
 128                 /* YUV formats */
 129                 .fourcc                 = V4L2_PIX_FMT_YUV420,
 130                 .depth                  = 8,
 131                 .bytesperline_align     = 32,
 132                 .flags                  = 0,
 133                 .mmal_fmt               = MMAL_ENCODING_I420,
 134                 .size_multiplier_x2     = 3,
 135         }, {
 136                 .fourcc                 = V4L2_PIX_FMT_YVU420,
 137                 .depth                  = 8,
 138                 .bytesperline_align     = 32,
 139                 .flags                  = 0,
 140                 .mmal_fmt               = MMAL_ENCODING_YV12,
 141                 .size_multiplier_x2     = 3,
 142         }, {
 143                 .fourcc                 = V4L2_PIX_FMT_NV12,
 144                 .depth                  = 8,
 145                 .bytesperline_align     = 32,
 146                 .flags                  = 0,
 147                 .mmal_fmt               = MMAL_ENCODING_NV12,
 148                 .size_multiplier_x2     = 3,
 149         }, {
 150                 .fourcc                 = V4L2_PIX_FMT_NV21,
 151                 .depth                  = 8,
 152                 .bytesperline_align     = 32,
 153                 .flags                  = 0,
 154                 .mmal_fmt               = MMAL_ENCODING_NV21,
 155                 .size_multiplier_x2     = 3,
 156         }, {
 157                 .fourcc                 = V4L2_PIX_FMT_RGB565,
 158                 .depth                  = 16,
 159                 .bytesperline_align     = 32,
 160                 .flags                  = 0,
 161                 .mmal_fmt               = MMAL_ENCODING_RGB16,
 162                 .size_multiplier_x2     = 2,
 163         }, {
 164                 .fourcc                 = V4L2_PIX_FMT_YUYV,
 165                 .depth                  = 16,
 166                 .bytesperline_align     = 32,
 167                 .flags                  = 0,
 168                 .mmal_fmt               = MMAL_ENCODING_YUYV,
 169                 .size_multiplier_x2     = 2,
 170         }, {
 171                 .fourcc                 = V4L2_PIX_FMT_UYVY,
 172                 .depth                  = 16,
 173                 .bytesperline_align     = 32,
 174                 .flags                  = 0,
 175                 .mmal_fmt               = MMAL_ENCODING_UYVY,
 176                 .size_multiplier_x2     = 2,
 177         }, {
 178                 .fourcc                 = V4L2_PIX_FMT_YVYU,
 179                 .depth                  = 16,
 180                 .bytesperline_align     = 32,
 181                 .flags                  = 0,
 182                 .mmal_fmt               = MMAL_ENCODING_YVYU,
 183                 .size_multiplier_x2     = 2,
 184         }, {
 185                 .fourcc                 = V4L2_PIX_FMT_VYUY,
 186                 .depth                  = 16,
 187                 .bytesperline_align     = 32,
 188                 .flags                  = 0,
 189                 .mmal_fmt               = MMAL_ENCODING_VYUY,
 190                 .size_multiplier_x2     = 2,
 191         }, {
 192                 /* RGB formats */
 193                 .fourcc                 = V4L2_PIX_FMT_RGB24,
 194                 .depth                  = 24,
 195                 .bytesperline_align     = 32,
 196                 .flags                  = 0,
 197                 .mmal_fmt               = MMAL_ENCODING_RGB24,
 198                 .size_multiplier_x2     = 2,
 199         }, {
 200                 .fourcc                 = V4L2_PIX_FMT_BGR24,
 201                 .depth                  = 24,
 202                 .bytesperline_align     = 32,
 203                 .flags                  = 0,
 204                 .mmal_fmt               = MMAL_ENCODING_BGR24,
 205                 .size_multiplier_x2     = 2,
 206         }, {
 207                 .fourcc                 = V4L2_PIX_FMT_BGR32,
 208                 .depth                  = 32,
 209                 .bytesperline_align     = 32,
 210                 .flags                  = 0,
 211                 .mmal_fmt               = MMAL_ENCODING_BGRA,
 212                 .size_multiplier_x2     = 2,
 213         }, {
 214                 /* Bayer formats */
 215                 /* 8 bit */
 216                 .fourcc                 = V4L2_PIX_FMT_SRGGB8,
 217                 .depth                  = 8,
 218                 .bytesperline_align     = 32,
 219                 .flags                  = 0,
 220                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB8,
 221                 .size_multiplier_x2     = 2,
 222                 .is_bayer               = true,
 223         }, {
 224                 .fourcc                 = V4L2_PIX_FMT_SBGGR8,
 225                 .depth                  = 8,
 226                 .bytesperline_align     = 32,
 227                 .flags                  = 0,
 228                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR8,
 229                 .size_multiplier_x2     = 2,
 230                 .is_bayer               = true,
 231         }, {
 232                 .fourcc                 = V4L2_PIX_FMT_SGRBG8,
 233                 .depth                  = 8,
 234                 .bytesperline_align     = 32,
 235                 .flags                  = 0,
 236                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG8,
 237                 .size_multiplier_x2     = 2,
 238                 .is_bayer               = true,
 239         }, {
 240                 .fourcc                 = V4L2_PIX_FMT_SGBRG8,
 241                 .depth                  = 8,
 242                 .bytesperline_align     = 32,
 243                 .flags                  = 0,
 244                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG8,
 245                 .size_multiplier_x2     = 2,
 246                 .is_bayer               = true,
 247         }, {
 248                 /* 10 bit */
 249                 .fourcc                 = V4L2_PIX_FMT_SRGGB10P,
 250                 .depth                  = 10,
 251                 .bytesperline_align     = 32,
 252                 .flags                  = 0,
 253                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB10P,
 254                 .size_multiplier_x2     = 2,
 255                 .is_bayer               = true,
 256         }, {
 257                 .fourcc                 = V4L2_PIX_FMT_SBGGR10P,
 258                 .depth                  = 10,
 259                 .bytesperline_align     = 32,
 260                 .flags                  = 0,
 261                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR10P,
 262                 .size_multiplier_x2     = 2,
 263                 .is_bayer               = true,
 264         }, {
 265                 .fourcc                 = V4L2_PIX_FMT_SGRBG10P,
 266                 .depth                  = 10,
 267                 .bytesperline_align     = 32,
 268                 .flags                  = 0,
 269                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG10P,
 270                 .size_multiplier_x2     = 2,
 271                 .is_bayer               = true,
 272         }, {
 273                 .fourcc                 = V4L2_PIX_FMT_SGBRG10P,
 274                 .depth                  = 10,
 275                 .bytesperline_align     = 32,
 276                 .flags                  = 0,
 277                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG10P,
 278                 .size_multiplier_x2     = 2,
 279                 .is_bayer               = true,
 280         }, {
 281                 /* 12 bit */
 282                 .fourcc                 = V4L2_PIX_FMT_SRGGB12P,
 283                 .depth                  = 12,
 284                 .bytesperline_align     = 32,
 285                 .flags                  = 0,
 286                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB12P,
 287                 .size_multiplier_x2     = 2,
 288                 .is_bayer               = true,
 289         }, {
 290                 .fourcc                 = V4L2_PIX_FMT_SBGGR12P,
 291                 .depth                  = 12,
 292                 .bytesperline_align     = 32,
 293                 .flags                  = 0,
 294                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR12P,
 295                 .size_multiplier_x2     = 2,
 296                 .is_bayer               = true,
 297         }, {
 298                 .fourcc                 = V4L2_PIX_FMT_SGRBG12P,
 299                 .depth                  = 12,
 300                 .bytesperline_align     = 32,
 301                 .flags                  = 0,
 302                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG12P,
 303                 .size_multiplier_x2     = 2,
 304                 .is_bayer               = true,
 305         }, {
 306                 .fourcc                 = V4L2_PIX_FMT_SGBRG12P,
 307                 .depth                  = 12,
 308                 .bytesperline_align     = 32,
 309                 .flags                  = 0,
 310                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG12P,
 311                 .size_multiplier_x2     = 2,
 312                 .is_bayer               = true,
 313         }, {
 314                 /* 14 bit */
 315                 .fourcc                 = V4L2_PIX_FMT_SRGGB14P,
 316                 .depth                  = 14,
 317                 .bytesperline_align     = 32,
 318                 .flags                  = 0,
 319                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB14P,
 320                 .size_multiplier_x2     = 2,
 321                 .is_bayer               = true,
 322         }, {
 323                 .fourcc                 = V4L2_PIX_FMT_SBGGR14P,
 324                 .depth                  = 14,
 325                 .bytesperline_align     = 32,
 326                 .flags                  = 0,
 327                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR14P,
 328                 .size_multiplier_x2     = 2,
 329                 .is_bayer               = true,
 330
 331         }, {
 332                 .fourcc                 = V4L2_PIX_FMT_SGRBG14P,
 333                 .depth                  = 14,
 334                 .bytesperline_align     = 32,
 335                 .flags                  = 0,
 336                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG14P,
 337                 .size_multiplier_x2     = 2,
 338                 .is_bayer               = true,
 339         }, {
 340                 .fourcc                 = V4L2_PIX_FMT_SGBRG14P,
 341                 .depth                  = 14,
 342                 .bytesperline_align     = 32,
 343                 .flags                  = 0,
 344                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG14P,
 345                 .size_multiplier_x2     = 2,
 346                 .is_bayer               = true,
 347         }, {
 348                 /* 16 bit */
 349                 .fourcc                 = V4L2_PIX_FMT_SRGGB16,
 350                 .depth                  = 16,
 351                 .bytesperline_align     = 32,
 352                 .flags                  = 0,
 353                 .mmal_fmt               = MMAL_ENCODING_BAYER_SRGGB16,
 354                 .size_multiplier_x2     = 2,
 355                 .is_bayer               = true,
 356         }, {
 357                 .fourcc                 = V4L2_PIX_FMT_SBGGR16,
 358                 .depth                  = 16,
 359                 .bytesperline_align     = 32,
 360                 .flags                  = 0,
 361                 .mmal_fmt               = MMAL_ENCODING_BAYER_SBGGR16,
 362                 .size_multiplier_x2     = 2,
 363                 .is_bayer               = true,
 364         }, {
 365                 .fourcc                 = V4L2_PIX_FMT_SGRBG16,
 366                 .depth                  = 16,
 367                 .bytesperline_align     = 32,
 368                 .flags                  = 0,
 369                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGRBG16,
 370                 .size_multiplier_x2     = 2,
 371                 .is_bayer               = true,
 372         }, {
 373                 .fourcc                 = V4L2_PIX_FMT_SGBRG16,
 374                 .depth                  = 16,
 375                 .bytesperline_align     = 32,
 376                 .flags                  = 0,
 377                 .mmal_fmt               = MMAL_ENCODING_BAYER_SGBRG16,
 378                 .size_multiplier_x2     = 2,
 379                 .is_bayer               = true,
 380         }, {
 381                 /* Monochrome MIPI formats */
 382                 /* 8 bit */
 383                 .fourcc                 = V4L2_PIX_FMT_GREY,
 384                 .depth                  = 8,
 385                 .bytesperline_align     = 32,
 386                 .flags                  = 0,
 387                 .mmal_fmt               = MMAL_ENCODING_GREY,
 388                 .size_multiplier_x2     = 2,
 389         }, {
 390                 /* 10 bit */
 391                 .fourcc                 = V4L2_PIX_FMT_Y10P,
 392                 .depth                  = 10,
 393                 .bytesperline_align     = 32,
 394                 .flags                  = 0,
 395                 .mmal_fmt               = MMAL_ENCODING_Y10P,
 396                 .size_multiplier_x2     = 2,
 397         }, {
 398                 /* 12 bit */
 399                 .fourcc                 = V4L2_PIX_FMT_Y12P,
 400                 .depth                  = 12,
 401                 .bytesperline_align     = 32,
 402                 .flags                  = 0,
 403                 .mmal_fmt               = MMAL_ENCODING_Y12P,
 404                 .size_multiplier_x2     = 2,
 405         }, {
 406                 /* 14 bit */
 407                 .fourcc                 = V4L2_PIX_FMT_Y14P,
 408                 .depth                  = 14,
 409                 .bytesperline_align     = 32,
 410                 .flags                  = 0,
 411                 .mmal_fmt               = MMAL_ENCODING_Y14P,
 412                 .size_multiplier_x2     = 2,
 413         }, {
 414                 /* 16 bit */
 415                 .fourcc                 = V4L2_PIX_FMT_Y16,
 416                 .depth                  = 16,
 417                 .bytesperline_align     = 32,
 418                 .flags                  = 0,
 419                 .mmal_fmt               = MMAL_ENCODING_Y16,
 420                 .size_multiplier_x2     = 2,
 421         }, {
 422                 /* Compressed formats */
 423                 .fourcc                 = V4L2_PIX_FMT_H264,
 424                 .depth                  = 0,
 425                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 426                 .mmal_fmt               = MMAL_ENCODING_H264,
 427         }, {
 428                 .fourcc                 = V4L2_PIX_FMT_MJPEG,
 429                 .depth                  = 0,
 430                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 431                 .mmal_fmt               = MMAL_ENCODING_MJPEG,
 432         }, {
 433                 .fourcc                 = V4L2_PIX_FMT_MPEG4,
 434                 .depth                  = 0,
 435                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 436                 .mmal_fmt               = MMAL_ENCODING_MP4V,
 437         }, {
 438                 .fourcc                 = V4L2_PIX_FMT_H263,
 439                 .depth                  = 0,
 440                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 441                 .mmal_fmt               = MMAL_ENCODING_H263,
 442         }, {
 443                 .fourcc                 = V4L2_PIX_FMT_MPEG2,
 444                 .depth                  = 0,
 445                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 446                 .mmal_fmt               = MMAL_ENCODING_MP2V,
 447         }, {
 448                 .fourcc                 = V4L2_PIX_FMT_VP8,
 449                 .depth                  = 0,
 450                 .flags                  = V4L2_FMT_FLAG_COMPRESSED,
 451                 .mmal_fmt               = MMAL_ENCODING_VP8,
 452         },
 453 };
 454
 455 struct bcm2835_codec_fmt_list {
 456         struct bcm2835_codec_fmt *list;
 457         unsigned int num_entries;
 458 };
 459
 460 struct m2m_mmal_buffer {
 461         struct v4l2_m2m_buffer  m2m;
 462         struct mmal_buffer      mmal;
 463 };
 464
 465 /* Per-queue, driver-specific private data */
 466 struct bcm2835_codec_q_data {
 467         /*
 468          * These parameters should be treated as gospel, with everything else
 469          * being determined from them.
 470          */
 471         /* Buffer width/height */
 472         unsigned int            bytesperline;
 473         unsigned int            height;
 474         /* Crop size used for selection handling */
 475         unsigned int            crop_width;
 476         unsigned int            crop_height;
 477         bool                    selection_set;
 478
 479         unsigned int            sizeimage;
 480         unsigned int            sequence;
 481         struct bcm2835_codec_fmt        *fmt;
 482
 483         /* One extra buffer header so we can send an EOS. */
 484         struct m2m_mmal_buffer  eos_buffer;
 485         bool                    eos_buffer_in_use;      /* debug only */
 486 };
 487
 488 struct bcm2835_codec_dev {
 489         struct platform_device *pdev;
 490
 491         /* v4l2 devices */
 492         struct v4l2_device      v4l2_dev;
 493         struct video_device     vfd;
 494         /* mutex for the v4l2 device */
 495         struct mutex            dev_mutex;
 496         atomic_t                num_inst;
 497
 498         /* allocated mmal instance and components */
 499         enum bcm2835_codec_role role;
 500         /* The list of formats supported on input and output queues. */
 501         struct bcm2835_codec_fmt_list   supported_fmts[2];
 502
 503         struct vchiq_mmal_instance      *instance;
 504
 505         struct v4l2_m2m_dev     *m2m_dev;
 506 };
 507
 508 struct bcm2835_codec_ctx {
 509         struct v4l2_fh          fh;
 510         struct bcm2835_codec_dev        *dev;
 511
 512         struct v4l2_ctrl_handler hdl;
 513
 514         struct vchiq_mmal_component  *component;
 515         bool component_enabled;
 516
 517         enum v4l2_colorspace    colorspace;
 518         enum v4l2_ycbcr_encoding ycbcr_enc;
 519         enum v4l2_xfer_func     xfer_func;
 520         enum v4l2_quantization  quant;
 521
 522         /* Source and destination queue data */
 523         struct bcm2835_codec_q_data   q_data[2];
 524         s32  bitrate;
 525         unsigned int    framerate_num;
 526         unsigned int    framerate_denom;
 527
 528         bool aborting;
 529         int num_ip_buffers;
 530         int num_op_buffers;
 531         struct completion frame_cmplt;
 532 };
 533
 534 struct bcm2835_codec_driver {
 535         struct platform_device *pdev;
 536         struct media_device     mdev;
 537
 538         struct bcm2835_codec_dev *encode;
 539         struct bcm2835_codec_dev *decode;
 540         struct bcm2835_codec_dev *isp;
 541 };
 542
 543 enum {
 544         V4L2_M2M_SRC = 0,
 545         V4L2_M2M_DST = 1,
 546 };
 547
 548 static const struct bcm2835_codec_fmt *get_fmt(u32 mmal_fmt)
 549 {
 550         unsigned int i;
 551
 552         for (i = 0; i < ARRAY_SIZE(supported_formats); i++) {
 553                 if (supported_formats[i].mmal_fmt == mmal_fmt &&
 554                     (!disable_bayer || !supported_formats[i].is_bayer))
 555                         return &supported_formats[i];
 556         }
 557         return NULL;
 558 }
 559
 560 static inline
 561 struct bcm2835_codec_fmt_list *get_format_list(struct bcm2835_codec_dev *dev,
 562                                                bool capture)
 563 {
 564         return &dev->supported_fmts[capture ? 1 : 0];
 565 }
 566
 567 static
 568 struct bcm2835_codec_fmt *get_default_format(struct bcm2835_codec_dev *dev,
 569                                              bool capture)
 570 {
 571         return &dev->supported_fmts[capture ? 1 : 0].list[0];
 572 }
 573
 574 static
 575 struct bcm2835_codec_fmt *find_format_pix_fmt(u32 pix_fmt,
 576                                               struct bcm2835_codec_dev *dev,
 577                                               bool capture)
 578 {
 579         struct bcm2835_codec_fmt *fmt;
 580         unsigned int k;
 581         struct bcm2835_codec_fmt_list *fmts =
 582                                         &dev->supported_fmts[capture ? 1 : 0];
 583
 584         for (k = 0; k < fmts->num_entries; k++) {
 585                 fmt = &fmts->list[k];
 586                 if (fmt->fourcc == pix_fmt)
 587                         break;
 588         }
 589         if (k == fmts->num_entries)
 590                 return NULL;
 591
 592         return &fmts->list[k];
 593 }
 594
 595 static inline
 596 struct bcm2835_codec_fmt *find_format(struct v4l2_format *f,
 597                                       struct bcm2835_codec_dev *dev,
 598                                       bool capture)
 599 {
 600         return find_format_pix_fmt(f->fmt.pix_mp.pixelformat, dev, capture);
 601 }
 602
 603 static inline struct bcm2835_codec_ctx *file2ctx(struct file *file)
 604 {
 605         return container_of(file->private_data, struct bcm2835_codec_ctx, fh);
 606 }
 607
 608 static struct bcm2835_codec_q_data *get_q_data(struct bcm2835_codec_ctx *ctx,
 609                                                enum v4l2_buf_type type)
 610 {
 611         switch (type) {
 612         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 613                 return &ctx->q_data[V4L2_M2M_SRC];
 614         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 615                 return &ctx->q_data[V4L2_M2M_DST];
 616         default:
 617                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Invalid queue type %u\n",
 618                          __func__, type);
 619                 break;
 620         }
 621         return NULL;
 622 }
 623
 624 static struct vchiq_mmal_port *get_port_data(struct bcm2835_codec_ctx *ctx,
 625                                              enum v4l2_buf_type type)
 626 {
 627         if (!ctx->component)
 628                 return NULL;
 629
 630         switch (type) {
 631         case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 632                 return &ctx->component->input[0];
 633         case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 634                 return &ctx->component->output[0];
 635         default:
 636                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Invalid queue type %u\n",
 637                          __func__, type);
 638                 break;
 639         }
 640         return NULL;
 641 }
 642
 643 /*
 644  * mem2mem callbacks
 645  */
 646
 647 /*
 648  * job_ready() - check whether an instance is ready to be scheduled to run
 649  */
 650 static int job_ready(void *priv)
 651 {
 652         struct bcm2835_codec_ctx *ctx = priv;
 653
 654         if (!v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) &&
 655             !v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx))
 656                 return 0;
 657
 658         return 1;
 659 }
 660
 661 static void job_abort(void *priv)
 662 {
 663         struct bcm2835_codec_ctx *ctx = priv;
 664
 665         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s\n", __func__);
 666         /* Will cancel the transaction in the next interrupt handler */
 667         ctx->aborting = 1;
 668 }
 669
 670 static inline unsigned int get_sizeimage(int bpl, int width, int height,
 671                                          struct bcm2835_codec_fmt *fmt)
 672 {
 673         if (fmt->flags & V4L2_FMT_FLAG_COMPRESSED) {
 674                 if (width * height > 1280 * 720)
 675                         return DEF_COMP_BUF_SIZE_GREATER_720P;
 676                 else
 677                         return DEF_COMP_BUF_SIZE_720P_OR_LESS;
 678         } else {
 679                 return (bpl * height * fmt->size_multiplier_x2) >> 1;
 680         }
 681 }
 682
 683 static inline unsigned int get_bytesperline(int width,
 684                                             struct bcm2835_codec_fmt *fmt)
 685 {
 686         return ALIGN((width * fmt->depth) >> 3, fmt->bytesperline_align);
 687 }
 688
 689 static void setup_mmal_port_format(struct bcm2835_codec_ctx *ctx,
 690                                    struct bcm2835_codec_q_data *q_data,
 691                                    struct vchiq_mmal_port *port)
 692 {
 693         port->format.encoding = q_data->fmt->mmal_fmt;
 694
 695         if (!(q_data->fmt->flags & V4L2_FMT_FLAG_COMPRESSED)) {
 696                 /* Raw image format - set width/height */
 697                 port->es.video.width = (q_data->bytesperline << 3) /
 698                                                 q_data->fmt->depth;
 699                 port->es.video.height = q_data->height;
 700                 port->es.video.crop.width = q_data->crop_width;
 701                 port->es.video.crop.height = q_data->crop_height;
 702                 port->es.video.frame_rate.num = ctx->framerate_num;
 703                 port->es.video.frame_rate.den = ctx->framerate_denom;
 704         } else {
 705                 /* Compressed format - leave resolution as 0 for decode */
 706                 if (ctx->dev->role == DECODE) {
 707                         port->es.video.width = 0;
 708                         port->es.video.height = 0;
 709                         port->es.video.crop.width = 0;
 710                         port->es.video.crop.height = 0;
 711                 } else {
 712                         port->es.video.width = q_data->crop_width;
 713                         port->es.video.height = q_data->height;
 714                         port->es.video.crop.width = q_data->crop_width;
 715                         port->es.video.crop.height = q_data->crop_height;
 716                         port->format.bitrate = ctx->bitrate;
 717                         port->es.video.frame_rate.num = ctx->framerate_num;
 718                         port->es.video.frame_rate.den = ctx->framerate_denom;
 719                 }
 720         }
 721         port->es.video.crop.x = 0;
 722         port->es.video.crop.y = 0;
 723
 724         port->current_buffer.size = q_data->sizeimage;
 725 };
 726
 727 static void ip_buffer_cb(struct vchiq_mmal_instance *instance,
 728                          struct vchiq_mmal_port *port, int status,
 729                          struct mmal_buffer *mmal_buf)
 730 {
 731         struct bcm2835_codec_ctx *ctx = port->cb_ctx/*, *curr_ctx*/;
 732         struct m2m_mmal_buffer *buf =
 733                         container_of(mmal_buf, struct m2m_mmal_buffer, mmal);
 734
 735         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: port %p buf %p length %lu, flags %x\n",
 736                  __func__, port, mmal_buf, mmal_buf->length,
 737                  mmal_buf->mmal_flags);
 738
 739         if (buf == &ctx->q_data[V4L2_M2M_SRC].eos_buffer) {
 740                 /* Do we need to add lcoking to prevent multiple submission of
 741                  * the EOS, and therefore handle mutliple return here?
 742                  */
 743                 v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: eos buffer returned.\n",
 744                          __func__);
 745                 ctx->q_data[V4L2_M2M_SRC].eos_buffer_in_use = false;
 746                 return;
 747         }
 748
 749         if (status) {
 750                 /* error in transfer */
 751                 if (buf)
 752                         /* there was a buffer with the error so return it */
 753                         vb2_buffer_done(&buf->m2m.vb.vb2_buf,
 754                                         VB2_BUF_STATE_ERROR);
 755                 return;
 756         }
 757         if (mmal_buf->cmd) {
 758                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Not expecting cmd msgs on ip callback - %08x\n",
 759                          __func__, mmal_buf->cmd);
 760                 /*
 761                  * CHECKME: Should we return here. The buffer shouldn't have a
 762                  * message context or vb2 buf associated.
 763                  */
 764         }
 765
 766         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: no error. Return buffer %p\n",
 767                  __func__, &buf->m2m.vb.vb2_buf);
 768         vb2_buffer_done(&buf->m2m.vb.vb2_buf, VB2_BUF_STATE_DONE);
 769
 770         ctx->num_ip_buffers++;
 771         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: done %d input buffers\n",
 772                  __func__, ctx->num_ip_buffers);
 773
 774         if (!port->enabled)
 775                 complete(&ctx->frame_cmplt);
 776 }
 777
 778 static void queue_res_chg_event(struct bcm2835_codec_ctx *ctx)
 779 {
 780         static const struct v4l2_event ev_src_ch = {
 781                 .type = V4L2_EVENT_SOURCE_CHANGE,
 782                 .u.src_change.changes =
 783                 V4L2_EVENT_SRC_CH_RESOLUTION,
 784         };
 785
 786         v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
 787 }
 788
 789 static void send_eos_event(struct bcm2835_codec_ctx *ctx)
 790 {
 791         static const struct v4l2_event ev = {
 792                 .type = V4L2_EVENT_EOS,
 793         };
 794
 795         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Sending EOS event\n");
 796
 797         v4l2_event_queue_fh(&ctx->fh, &ev);
 798 }
 799
 800 static void color_mmal2v4l(struct bcm2835_codec_ctx *ctx, u32 mmal_color_space)
 801 {
 802         switch (mmal_color_space) {
 803         case MMAL_COLOR_SPACE_ITUR_BT601:
 804                 ctx->colorspace = V4L2_COLORSPACE_REC709;
 805                 ctx->xfer_func = V4L2_XFER_FUNC_709;
 806                 ctx->ycbcr_enc = V4L2_YCBCR_ENC_601;
 807                 ctx->quant = V4L2_QUANTIZATION_LIM_RANGE;
 808                 break;
 809
 810         case MMAL_COLOR_SPACE_ITUR_BT709:
 811                 ctx->colorspace = V4L2_COLORSPACE_REC709;
 812                 ctx->xfer_func = V4L2_XFER_FUNC_709;
 813                 ctx->ycbcr_enc = V4L2_YCBCR_ENC_709;
 814                 ctx->quant = V4L2_QUANTIZATION_LIM_RANGE;
 815                 break;
 816         }
 817 }
 818
 819 static void handle_fmt_changed(struct bcm2835_codec_ctx *ctx,
 820                                struct mmal_buffer *mmal_buf)
 821 {
 822         struct bcm2835_codec_q_data *q_data;
 823         struct mmal_msg_event_format_changed *format =
 824                 (struct mmal_msg_event_format_changed *)mmal_buf->buffer;
 825         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format changed: buff size min %u, rec %u, buff num min %u, rec %u\n",
 826                  __func__,
 827                  format->buffer_size_min,
 828                  format->buffer_size_recommended,
 829                  format->buffer_num_min,
 830                  format->buffer_num_recommended
 831                 );
 832         if (format->format.type != MMAL_ES_TYPE_VIDEO) {
 833                 v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format changed but not video %u\n",
 834                          __func__, format->format.type);
 835                 return;
 836         }
 837         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format changed to %ux%u, crop %ux%u, colourspace %08X\n",
 838                  __func__, format->es.video.width, format->es.video.height,
 839                  format->es.video.crop.width, format->es.video.crop.height,
 840                  format->es.video.color_space);
 841
 842         q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
 843         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Format was %ux%u, crop %ux%u\n",
 844                  __func__, q_data->bytesperline, q_data->height,
 845                  q_data->crop_width, q_data->crop_height);
 846
 847         q_data->crop_width = format->es.video.crop.width;
 848         q_data->crop_height = format->es.video.crop.height;
 849         q_data->bytesperline = get_bytesperline(format->es.video.width,
 850                                                 q_data->fmt);
 851
 852         q_data->height = format->es.video.height;
 853         q_data->sizeimage = format->buffer_size_min;
 854         if (format->es.video.color_space)
 855                 color_mmal2v4l(ctx, format->es.video.color_space);
 856
 857         queue_res_chg_event(ctx);
 858 }
 859
 860 static void op_buffer_cb(struct vchiq_mmal_instance *instance,
 861                          struct vchiq_mmal_port *port, int status,
 862                          struct mmal_buffer *mmal_buf)
 863 {
 864         struct bcm2835_codec_ctx *ctx = port->cb_ctx;
 865         struct m2m_mmal_buffer *buf;
 866         struct vb2_v4l2_buffer *vb2;
 867
 868         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev,
 869                  "%s: status:%d, buf:%p, length:%lu, flags %u, pts %lld\n",
 870                  __func__, status, mmal_buf, mmal_buf->length,
 871                  mmal_buf->mmal_flags, mmal_buf->pts);
 872
 873         buf = container_of(mmal_buf, struct m2m_mmal_buffer, mmal);
 874         vb2 = &buf->m2m.vb;
 875
 876         if (status) {
 877                 /* error in transfer */
 878                 if (vb2) {
 879                         /* there was a buffer with the error so return it */
 880                         vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_ERROR);
 881                 }
 882                 return;
 883         }
 884
 885         if (mmal_buf->cmd) {
 886                 switch (mmal_buf->cmd) {
 887                 case MMAL_EVENT_FORMAT_CHANGED:
 888                 {
 889                         handle_fmt_changed(ctx, mmal_buf);
 890                         break;
 891                 }
 892                 default:
 893                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Unexpected event on output callback - %08x\n",
 894                                  __func__, mmal_buf->cmd);
 895                         break;
 896                 }
 897                 return;
 898         }
 899
 900         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: length %lu, flags %x, idx %u\n",
 901                  __func__, mmal_buf->length, mmal_buf->mmal_flags,
 902                  vb2->vb2_buf.index);
 903
 904         if (mmal_buf->length == 0) {
 905                 /* stream ended, or buffer being returned during disable. */
 906                 v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: Empty buffer - flags %04x",
 907                          __func__, mmal_buf->mmal_flags);
 908                 if (!mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS) {
 909                         vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_ERROR);
 910                         if (!port->enabled)
 911                                 complete(&ctx->frame_cmplt);
 912                         return;
 913                 }
 914         }
 915         if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS) {
 916                 /* EOS packet from the VPU */
 917                 send_eos_event(ctx);
 918                 vb2->flags |= V4L2_BUF_FLAG_LAST;
 919         }
 920
 921         /* vb2 timestamps in nsecs, mmal in usecs */
 922         vb2->vb2_buf.timestamp = mmal_buf->pts * 1000;
 923
 924         vb2_set_plane_payload(&vb2->vb2_buf, 0, mmal_buf->length);
 925         if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
 926                 vb2->flags |= V4L2_BUF_FLAG_KEYFRAME;
 927
 928         vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_DONE);
 929         ctx->num_op_buffers++;
 930
 931         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: done %d output buffers\n",
 932                  __func__, ctx->num_op_buffers);
 933
 934         if (!port->enabled)
 935                 complete(&ctx->frame_cmplt);
 936 }
 937
 938 /* vb2_to_mmal_buffer() - converts vb2 buffer header to MMAL
 939  *
 940  * Copies all the required fields from a VB2 buffer to the MMAL buffer header,
 941  * ready for sending to the VPU.
 942  */
 943 static void vb2_to_mmal_buffer(struct m2m_mmal_buffer *buf,
 944                                struct vb2_v4l2_buffer *vb2)
 945 {
 946         u64 pts;
 947         buf->mmal.mmal_flags = 0;
 948         if (vb2->flags & V4L2_BUF_FLAG_KEYFRAME)
 949                 buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_FLAG_KEYFRAME;
 950
 951         /*
 952          * Adding this means that the data must be framed correctly as one frame
 953          * per buffer. The underlying decoder has no such requirement, but it
 954          * will reduce latency as the bistream parser will be kicked immediately
 955          * to parse the frame, rather than relying on its own heuristics for
 956          * when to wake up.
 957          */
 958         buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_FLAG_FRAME_END;
 959
 960         buf->mmal.length = vb2->vb2_buf.planes[0].bytesused;
 961         /*
 962          * Minor ambiguity in the V4L2 spec as to whether passing in a 0 length
 963          * buffer, or one with V4L2_BUF_FLAG_LAST set denotes end of stream.
 964          * Handle either.
 965          */
 966         if (!buf->mmal.length || vb2->flags & V4L2_BUF_FLAG_LAST)
 967                 buf->mmal.mmal_flags |= MMAL_BUFFER_HEADER_FLAG_EOS;
 968
 969         /* vb2 timestamps in nsecs, mmal in usecs */
 970         pts = vb2->vb2_buf.timestamp;
 971         do_div(pts, 1000);
 972         buf->mmal.pts = pts;
 973         buf->mmal.dts = MMAL_TIME_UNKNOWN;
 974 }
 975
 976 /* device_run() - prepares and starts the device
 977  *
 978  * This simulates all the immediate preparations required before starting
 979  * a device. This will be called by the framework when it decides to schedule
 980  * a particular instance.
 981  */
 982 static void device_run(void *priv)
 983 {
 984         struct bcm2835_codec_ctx *ctx = priv;
 985         struct bcm2835_codec_dev *dev = ctx->dev;
 986         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 987         struct m2m_mmal_buffer *src_m2m_buf = NULL, *dst_m2m_buf = NULL;
 988         struct v4l2_m2m_buffer *m2m;
 989         int ret;
 990
 991         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: off we go\n", __func__);
 992
 993         src_buf = v4l2_m2m_buf_remove(&ctx->fh.m2m_ctx->out_q_ctx);
 994         if (src_buf) {
 995                 m2m = container_of(src_buf, struct v4l2_m2m_buffer, vb);
 996                 src_m2m_buf = container_of(m2m, struct m2m_mmal_buffer, m2m);
 997                 vb2_to_mmal_buffer(src_m2m_buf, src_buf);
 998
 999                 ret = vchiq_mmal_submit_buffer(dev->instance,
1000                                                &ctx->component->input[0],
1001                                                &src_m2m_buf->mmal);
1002                 v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: Submitted ip buffer len %lu, pts %llu, flags %04x\n",
1003                          __func__, src_m2m_buf->mmal.length,
1004                          src_m2m_buf->mmal.pts, src_m2m_buf->mmal.mmal_flags);
1005                 if (ret)
1006                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed submitting ip buffer\n",
1007                                  __func__);
1008         }
1009
1010         dst_buf = v4l2_m2m_buf_remove(&ctx->fh.m2m_ctx->cap_q_ctx);
1011         if (dst_buf) {
1012                 m2m = container_of(dst_buf, struct v4l2_m2m_buffer, vb);
1013                 dst_m2m_buf = container_of(m2m, struct m2m_mmal_buffer, m2m);
1014                 vb2_to_mmal_buffer(dst_m2m_buf, dst_buf);
1015
1016                 ret = vchiq_mmal_submit_buffer(dev->instance,
1017                                                &ctx->component->output[0],
1018                                                &dst_m2m_buf->mmal);
1019                 if (ret)
1020                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed submitting op buffer\n",
1021                                  __func__);
1022         }
1023
1024         v4l2_dbg(3, debug, &ctx->dev->v4l2_dev, "%s: Submitted src %p, dst %p\n",
1025                  __func__, src_m2m_buf, dst_m2m_buf);
1026
1027         /* Complete the job here. */
1028         v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
1029 }
1030
1031 /*
1032  * video ioctls
1033  */
1034 static int vidioc_querycap(struct file *file, void *priv,
1035                            struct v4l2_capability *cap)
1036 {
1037         struct bcm2835_codec_dev *dev = video_drvdata(file);
1038
1039         strncpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver) - 1);
1040         strncpy(cap->card, dev->vfd.name, sizeof(cap->card) - 1);
1041         snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
1042                  MEM2MEM_NAME);
1043         return 0;
1044 }
1045
1046 static int enum_fmt(struct v4l2_fmtdesc *f, struct bcm2835_codec_ctx *ctx,
1047                     bool capture)
1048 {
1049         struct bcm2835_codec_fmt *fmt;
1050         struct bcm2835_codec_fmt_list *fmts =
1051                                         get_format_list(ctx->dev, capture);
1052
1053         if (f->index < fmts->num_entries) {
1054                 /* Format found */
1055                 fmt = &fmts->list[f->index];
1056                 f->pixelformat = fmt->fourcc;
1057                 f->flags = fmt->flags;
1058                 return 0;
1059         }
1060
1061         /* Format not found */
1062         return -EINVAL;
1063 }
1064
1065 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
1066                                    struct v4l2_fmtdesc *f)
1067 {
1068         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1069
1070         return enum_fmt(f, ctx, true);
1071 }
1072
1073 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
1074                                    struct v4l2_fmtdesc *f)
1075 {
1076         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1077
1078         return enum_fmt(f, ctx, false);
1079 }
1080
1081 static int vidioc_g_fmt(struct bcm2835_codec_ctx *ctx, struct v4l2_format *f)
1082 {
1083         struct vb2_queue *vq;
1084         struct bcm2835_codec_q_data *q_data;
1085
1086         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
1087         if (!vq)
1088                 return -EINVAL;
1089
1090         q_data = get_q_data(ctx, f->type);
1091
1092         f->fmt.pix_mp.width                     = q_data->crop_width;
1093         f->fmt.pix_mp.height                    = q_data->height;
1094         f->fmt.pix_mp.pixelformat               = q_data->fmt->fourcc;
1095         f->fmt.pix_mp.field                     = V4L2_FIELD_NONE;
1096         f->fmt.pix_mp.colorspace                = ctx->colorspace;
1097         f->fmt.pix_mp.plane_fmt[0].sizeimage    = q_data->sizeimage;
1098         f->fmt.pix_mp.plane_fmt[0].bytesperline = q_data->bytesperline;
1099         f->fmt.pix_mp.num_planes                = 1;
1100         f->fmt.pix_mp.ycbcr_enc                 = ctx->ycbcr_enc;
1101         f->fmt.pix_mp.quantization              = ctx->quant;
1102         f->fmt.pix_mp.xfer_func                 = ctx->xfer_func;
1103
1104         memset(f->fmt.pix_mp.plane_fmt[0].reserved, 0,
1105                sizeof(f->fmt.pix_mp.plane_fmt[0].reserved));
1106
1107         return 0;
1108 }
1109
1110 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
1111                                 struct v4l2_format *f)
1112 {
1113         return vidioc_g_fmt(file2ctx(file), f);
1114 }
1115
1116 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
1117                                 struct v4l2_format *f)
1118 {
1119         return vidioc_g_fmt(file2ctx(file), f);
1120 }
1121
1122 static int vidioc_try_fmt(struct bcm2835_codec_ctx *ctx, struct v4l2_format *f,
1123                           struct bcm2835_codec_fmt *fmt)
1124 {
1125         /*
1126          * The V4L2 specification requires the driver to correct the format
1127          * struct if any of the dimensions is unsupported
1128          */
1129         if (f->fmt.pix_mp.width > MAX_W)
1130                 f->fmt.pix_mp.width = MAX_W;
1131         if (f->fmt.pix_mp.height > MAX_H)
1132                 f->fmt.pix_mp.height = MAX_H;
1133
1134         if (!fmt->flags & V4L2_FMT_FLAG_COMPRESSED) {
1135                 /* Only clip min w/h on capture. Treat 0x0 as unknown. */
1136                 if (f->fmt.pix_mp.width < MIN_W)
1137                         f->fmt.pix_mp.width = MIN_W;
1138                 if (f->fmt.pix_mp.height < MIN_H)
1139                         f->fmt.pix_mp.height = MIN_H;
1140
1141                 /*
1142                  * For decoders the buffer must have a vertical alignment of 16
1143                  * lines.
1144                  * The selection will reflect any cropping rectangle when only
1145                  * some of the pixels are active.
1146                  */
1147                 if (ctx->dev->role == DECODE)
1148                         f->fmt.pix_mp.height = ALIGN(f->fmt.pix_mp.height, 16);
1149         }
1150         f->fmt.pix_mp.num_planes = 1;
1151         f->fmt.pix_mp.plane_fmt[0].bytesperline =
1152                 get_bytesperline(f->fmt.pix_mp.width, fmt);
1153         f->fmt.pix_mp.plane_fmt[0].sizeimage =
1154                 get_sizeimage(f->fmt.pix_mp.plane_fmt[0].bytesperline,
1155                               f->fmt.pix_mp.width, f->fmt.pix_mp.height, fmt);
1156         memset(f->fmt.pix_mp.plane_fmt[0].reserved, 0,
1157                sizeof(f->fmt.pix_mp.plane_fmt[0].reserved));
1158
1159         f->fmt.pix_mp.field = V4L2_FIELD_NONE;
1160
1161         return 0;
1162 }
1163
1164 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
1165                                   struct v4l2_format *f)
1166 {
1167         struct bcm2835_codec_fmt *fmt;
1168         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1169
1170         fmt = find_format(f, ctx->dev, true);
1171         if (!fmt) {
1172                 f->fmt.pix_mp.pixelformat = get_default_format(ctx->dev,
1173                                                                true)->fourcc;
1174                 fmt = find_format(f, ctx->dev, true);
1175         }
1176
1177         return vidioc_try_fmt(ctx, f, fmt);
1178 }
1179
1180 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
1181                                   struct v4l2_format *f)
1182 {
1183         struct bcm2835_codec_fmt *fmt;
1184         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1185
1186         fmt = find_format(f, ctx->dev, false);
1187         if (!fmt) {
1188                 f->fmt.pix_mp.pixelformat = get_default_format(ctx->dev,
1189                                                                false)->fourcc;
1190                 fmt = find_format(f, ctx->dev, false);
1191         }
1192
1193         if (!f->fmt.pix_mp.colorspace)
1194                 f->fmt.pix_mp.colorspace = ctx->colorspace;
1195
1196         return vidioc_try_fmt(ctx, f, fmt);
1197 }
1198
1199 static int vidioc_s_fmt(struct bcm2835_codec_ctx *ctx, struct v4l2_format *f,
1200                         unsigned int requested_height)
1201 {
1202         struct bcm2835_codec_q_data *q_data;
1203         struct vb2_queue *vq;
1204         struct vchiq_mmal_port *port;
1205         bool update_capture_port = false;
1206         int ret;
1207
1208         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Setting format for type %d, wxh: %dx%d, fmt: " V4L2_FOURCC_CONV ", size %u\n",
1209                  f->type, f->fmt.pix_mp.width, f->fmt.pix_mp.height,
1210                  V4L2_FOURCC_CONV_ARGS(f->fmt.pix_mp.pixelformat),
1211                  f->fmt.pix_mp.plane_fmt[0].sizeimage);
1212
1213         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
1214         if (!vq)
1215                 return -EINVAL;
1216
1217         q_data = get_q_data(ctx, f->type);
1218         if (!q_data)
1219                 return -EINVAL;
1220
1221         if (vb2_is_busy(vq)) {
1222                 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
1223                 return -EBUSY;
1224         }
1225
1226         q_data->fmt = find_format(f, ctx->dev,
1227                                   f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
1228         q_data->crop_width = f->fmt.pix_mp.width;
1229         q_data->height = f->fmt.pix_mp.height;
1230         if (!q_data->selection_set)
1231                 q_data->crop_height = requested_height;
1232
1233         /*
1234          * Copying the behaviour of vicodec which retains a single set of
1235          * colorspace parameters for both input and output.
1236          */
1237         ctx->colorspace = f->fmt.pix_mp.colorspace;
1238         ctx->xfer_func = f->fmt.pix_mp.xfer_func;
1239         ctx->ycbcr_enc = f->fmt.pix_mp.ycbcr_enc;
1240         ctx->quant = f->fmt.pix_mp.quantization;
1241
1242         /* All parameters should have been set correctly by try_fmt */
1243         q_data->bytesperline = f->fmt.pix_mp.plane_fmt[0].bytesperline;
1244         q_data->sizeimage = f->fmt.pix_mp.plane_fmt[0].sizeimage;
1245
1246         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Calulated bpl as %u, size %u\n",
1247                  q_data->bytesperline, q_data->sizeimage);
1248
1249         if (ctx->dev->role == DECODE &&
1250             q_data->fmt->flags & V4L2_FMT_FLAG_COMPRESSED &&
1251             q_data->crop_width && q_data->height) {
1252                 /*
1253                  * On the decoder, if provided with a resolution on the input
1254                  * side, then replicate that to the output side.
1255                  * GStreamer appears not to support V4L2_EVENT_SOURCE_CHANGE,
1256                  * nor set up a resolution on the output side, therefore
1257                  * we can't decode anything at a resolution other than the
1258                  * default one.
1259                  */
1260                 struct bcm2835_codec_q_data *q_data_dst =
1261                                                 &ctx->q_data[V4L2_M2M_DST];
1262
1263                 q_data_dst->crop_width = q_data->crop_width;
1264                 q_data_dst->crop_height = q_data->crop_height;
1265                 q_data_dst->height = ALIGN(q_data->crop_height, 16);
1266
1267                 q_data_dst->bytesperline =
1268                         get_bytesperline(f->fmt.pix_mp.width, q_data_dst->fmt);
1269                 q_data_dst->sizeimage = get_sizeimage(q_data_dst->bytesperline,
1270                                                       q_data_dst->crop_width,
1271                                                       q_data_dst->height,
1272                                                       q_data_dst->fmt);
1273                 update_capture_port = true;
1274         }
1275
1276         /* If we have a component then setup the port as well */
1277         port = get_port_data(ctx, vq->type);
1278         if (!port)
1279                 return 0;
1280
1281         setup_mmal_port_format(ctx, q_data, port);
1282         ret = vchiq_mmal_port_set_format(ctx->dev->instance, port);
1283         if (ret) {
1284                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed vchiq_mmal_port_set_format on port, ret %d\n",
1285                          __func__, ret);
1286                 ret = -EINVAL;
1287         }
1288
1289         if (q_data->sizeimage < port->minimum_buffer.size) {
1290                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Current buffer size of %u < min buf size %u - driver mismatch to MMAL\n",
1291                          __func__, q_data->sizeimage,
1292                          port->minimum_buffer.size);
1293         }
1294
1295         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "Set format for type %d, wxh: %dx%d, fmt: %08x, size %u\n",
1296                  f->type, q_data->crop_width, q_data->height,
1297                  q_data->fmt->fourcc, q_data->sizeimage);
1298
1299         if (update_capture_port) {
1300                 struct vchiq_mmal_port *port_dst = &ctx->component->output[0];
1301                 struct bcm2835_codec_q_data *q_data_dst =
1302                                                 &ctx->q_data[V4L2_M2M_DST];
1303
1304                 setup_mmal_port_format(ctx, q_data_dst, port_dst);
1305                 ret = vchiq_mmal_port_set_format(ctx->dev->instance, port_dst);
1306                 if (ret) {
1307                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed vchiq_mmal_port_set_format on output port, ret %d\n",
1308                                  __func__, ret);
1309                         ret = -EINVAL;
1310                 }
1311         }
1312         return ret;
1313 }
1314
1315 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
1316                                 struct v4l2_format *f)
1317 {
1318         unsigned int height = f->fmt.pix_mp.height;
1319         int ret;
1320
1321         ret = vidioc_try_fmt_vid_cap(file, priv, f);
1322         if (ret)
1323                 return ret;
1324
1325         return vidioc_s_fmt(file2ctx(file), f, height);
1326 }
1327
1328 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
1329                                 struct v4l2_format *f)
1330 {
1331         unsigned int height = f->fmt.pix_mp.height;
1332         int ret;
1333
1334         ret = vidioc_try_fmt_vid_out(file, priv, f);
1335         if (ret)
1336                 return ret;
1337
1338         ret = vidioc_s_fmt(file2ctx(file), f, height);
1339         return ret;
1340 }
1341
1342 static int vidioc_g_selection(struct file *file, void *priv,
1343                               struct v4l2_selection *s)
1344 {
1345         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1346         struct bcm2835_codec_q_data *q_data;
1347
1348         /*
1349          * The selection API takes V4L2_BUF_TYPE_VIDEO_CAPTURE and
1350          * V4L2_BUF_TYPE_VIDEO_OUTPUT, even if the device implements the MPLANE
1351          * API. The V4L2 core will have converted the MPLANE variants to
1352          * non-MPLANE.
1353          * Open code this instead of using get_q_data in this case.
1354          */
1355         switch (s->type) {
1356         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1357                 /* CAPTURE on encoder is not valid. */
1358                 if (ctx->dev->role == ENCODE)
1359                         return -EINVAL;
1360                 q_data = &ctx->q_data[V4L2_M2M_DST];
1361                 break;
1362         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1363                 /* OUTPUT on deoder is not valid. */
1364                 if (ctx->dev->role == DECODE)
1365                         return -EINVAL;
1366                 q_data = &ctx->q_data[V4L2_M2M_SRC];
1367                 break;
1368         default:
1369                 return -EINVAL;
1370         }
1371
1372         switch (ctx->dev->role) {
1373         case DECODE:
1374                 switch (s->target) {
1375                 case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1376                 case V4L2_SEL_TGT_COMPOSE:
1377                         s->r.left = 0;
1378                         s->r.top = 0;
1379                         s->r.width = q_data->crop_width;
1380                         s->r.height = q_data->crop_height;
1381                         break;
1382                 case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1383                         s->r.left = 0;
1384                         s->r.top = 0;
1385                         s->r.width = q_data->crop_width;
1386                         s->r.height = q_data->crop_height;
1387                         break;
1388                 default:
1389                         return -EINVAL;
1390                 }
1391                 break;
1392         case ENCODE:
1393                 switch (s->target) {
1394                 case V4L2_SEL_TGT_CROP_DEFAULT:
1395                 case V4L2_SEL_TGT_CROP_BOUNDS:
1396                         s->r.top = 0;
1397                         s->r.left = 0;
1398                         s->r.width = q_data->bytesperline;
1399                         s->r.height = q_data->height;
1400                         break;
1401                 case V4L2_SEL_TGT_CROP:
1402                         s->r.top = 0;
1403                         s->r.left = 0;
1404                         s->r.width = q_data->crop_width;
1405                         s->r.height = q_data->crop_height;
1406                         break;
1407                 default:
1408                         return -EINVAL;
1409                 }
1410                 break;
1411         case ISP:
1412                 break;
1413         }
1414
1415         return 0;
1416 }
1417
1418 static int vidioc_s_selection(struct file *file, void *priv,
1419                               struct v4l2_selection *s)
1420 {
1421         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1422         struct bcm2835_codec_q_data *q_data = NULL;
1423
1424         /*
1425          * The selection API takes V4L2_BUF_TYPE_VIDEO_CAPTURE and
1426          * V4L2_BUF_TYPE_VIDEO_OUTPUT, even if the device implements the MPLANE
1427          * API. The V4L2 core will have converted the MPLANE variants to
1428          * non-MPLANE.
1429          *
1430          * Open code this instead of using get_q_data in this case.
1431          */
1432         switch (s->type) {
1433         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1434                 /* CAPTURE on encoder is not valid. */
1435                 if (ctx->dev->role == ENCODE)
1436                         return -EINVAL;
1437                 q_data = &ctx->q_data[V4L2_M2M_DST];
1438                 break;
1439         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1440                 /* OUTPUT on deoder is not valid. */
1441                 if (ctx->dev->role == DECODE)
1442                         return -EINVAL;
1443                 q_data = &ctx->q_data[V4L2_M2M_SRC];
1444                 break;
1445         default:
1446                 return -EINVAL;
1447         }
1448
1449         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",
1450                  __func__, ctx, s->type, q_data, s->target, s->r.left, s->r.top,
1451                  s->r.width, s->r.height);
1452
1453         switch (ctx->dev->role) {
1454         case DECODE:
1455                 switch (s->target) {
1456                 case V4L2_SEL_TGT_COMPOSE:
1457                         /* Accept cropped image */
1458                         s->r.left = 0;
1459                         s->r.top = 0;
1460                         s->r.width = min(s->r.width, q_data->crop_width);
1461                         s->r.height = min(s->r.height, q_data->height);
1462                         q_data->crop_width = s->r.width;
1463                         q_data->crop_height = s->r.height;
1464                         q_data->selection_set = true;
1465                         break;
1466                 default:
1467                         return -EINVAL;
1468                 }
1469                 break;
1470         case ENCODE:
1471                 switch (s->target) {
1472                 case V4L2_SEL_TGT_CROP:
1473                         /* Only support crop from (0,0) */
1474                         s->r.top = 0;
1475                         s->r.left = 0;
1476                         s->r.width = min(s->r.width, q_data->crop_width);
1477                         s->r.height = min(s->r.height, q_data->crop_height);
1478                         q_data->crop_width = s->r.width;
1479                         q_data->crop_height = s->r.height;
1480                         q_data->selection_set = true;
1481                         break;
1482                 default:
1483                         return -EINVAL;
1484                 }
1485                 break;
1486         case ISP:
1487                 break;
1488         }
1489
1490         return 0;
1491 }
1492
1493 static int vidioc_s_parm(struct file *file, void *priv,
1494                          struct v4l2_streamparm *parm)
1495 {
1496         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1497
1498         if (parm->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
1499                 return -EINVAL;
1500
1501         if (!parm->parm.output.timeperframe.denominator ||
1502             !parm->parm.output.timeperframe.numerator)
1503                 return -EINVAL;
1504
1505         ctx->framerate_num =
1506                         parm->parm.output.timeperframe.denominator;
1507         ctx->framerate_denom =
1508                         parm->parm.output.timeperframe.numerator;
1509
1510         parm->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
1511
1512         return 0;
1513 }
1514
1515 static int vidioc_g_parm(struct file *file, void *priv,
1516                          struct v4l2_streamparm *parm)
1517 {
1518         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1519
1520         if (parm->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
1521                 return -EINVAL;
1522
1523         parm->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
1524         parm->parm.output.timeperframe.denominator =
1525                         ctx->framerate_num;
1526         parm->parm.output.timeperframe.numerator =
1527                         ctx->framerate_denom;
1528
1529         return 0;
1530 }
1531
1532 static int vidioc_subscribe_evt(struct v4l2_fh *fh,
1533                                 const struct v4l2_event_subscription *sub)
1534 {
1535         switch (sub->type) {
1536         case V4L2_EVENT_EOS:
1537                 return v4l2_event_subscribe(fh, sub, 2, NULL);
1538         case V4L2_EVENT_SOURCE_CHANGE:
1539                 return v4l2_src_change_event_subscribe(fh, sub);
1540         default:
1541                 return v4l2_ctrl_subscribe_event(fh, sub);
1542         }
1543 }
1544
1545 static int bcm2835_codec_set_level_profile(struct bcm2835_codec_ctx *ctx,
1546                                            struct v4l2_ctrl *ctrl)
1547 {
1548         struct mmal_parameter_video_profile param;
1549         int param_size = sizeof(param);
1550         int ret;
1551
1552         /*
1553          * Level and Profile are set via the same MMAL parameter.
1554          * Retrieve the current settings and amend the one that has changed.
1555          */
1556         ret = vchiq_mmal_port_parameter_get(ctx->dev->instance,
1557                                             &ctx->component->output[0],
1558                                             MMAL_PARAMETER_PROFILE,
1559                                             &param,
1560                                             &param_size);
1561         if (ret)
1562                 return ret;
1563
1564         switch (ctrl->id) {
1565         case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
1566                 switch (ctrl->val) {
1567                 case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
1568                         param.profile = MMAL_VIDEO_PROFILE_H264_BASELINE;
1569                         break;
1570                 case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
1571                         param.profile =
1572                                 MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE;
1573                         break;
1574                 case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
1575                         param.profile = MMAL_VIDEO_PROFILE_H264_MAIN;
1576                         break;
1577                 case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
1578                         param.profile = MMAL_VIDEO_PROFILE_H264_HIGH;
1579                         break;
1580                 default:
1581                         /* Should never get here */
1582                         break;
1583                 }
1584                 break;
1585
1586         case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
1587                 switch (ctrl->val) {
1588                 case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
1589                         param.level = MMAL_VIDEO_LEVEL_H264_1;
1590                         break;
1591                 case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
1592                         param.level = MMAL_VIDEO_LEVEL_H264_1b;
1593                         break;
1594                 case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
1595                         param.level = MMAL_VIDEO_LEVEL_H264_11;
1596                         break;
1597                 case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
1598                         param.level = MMAL_VIDEO_LEVEL_H264_12;
1599                         break;
1600                 case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
1601                         param.level = MMAL_VIDEO_LEVEL_H264_13;
1602                         break;
1603                 case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
1604                         param.level = MMAL_VIDEO_LEVEL_H264_2;
1605                         break;
1606                 case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
1607                         param.level = MMAL_VIDEO_LEVEL_H264_21;
1608                         break;
1609                 case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
1610                         param.level = MMAL_VIDEO_LEVEL_H264_22;
1611                         break;
1612                 case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
1613                         param.level = MMAL_VIDEO_LEVEL_H264_3;
1614                         break;
1615                 case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
1616                         param.level = MMAL_VIDEO_LEVEL_H264_31;
1617                         break;
1618                 case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
1619                         param.level = MMAL_VIDEO_LEVEL_H264_32;
1620                         break;
1621                 case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
1622                         param.level = MMAL_VIDEO_LEVEL_H264_4;
1623                         break;
1624                 default:
1625                         /* Should never get here */
1626                         break;
1627                 }
1628         }
1629         ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1630                                             &ctx->component->output[0],
1631                                             MMAL_PARAMETER_PROFILE,
1632                                             &param,
1633                                             param_size);
1634
1635         return ret;
1636 }
1637
1638 static int bcm2835_codec_s_ctrl(struct v4l2_ctrl *ctrl)
1639 {
1640         struct bcm2835_codec_ctx *ctx =
1641                 container_of(ctrl->handler, struct bcm2835_codec_ctx, hdl);
1642         int ret = 0;
1643
1644         switch (ctrl->id) {
1645         case V4L2_CID_MPEG_VIDEO_BITRATE:
1646                 ctx->bitrate = ctrl->val;
1647                 if (!ctx->component)
1648                         break;
1649
1650                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1651                                                     &ctx->component->output[0],
1652                                                     MMAL_PARAMETER_VIDEO_BIT_RATE,
1653                                                     &ctrl->val,
1654                                                     sizeof(ctrl->val));
1655                 break;
1656
1657         case V4L2_CID_MPEG_VIDEO_BITRATE_MODE: {
1658                 u32 bitrate_mode;
1659
1660                 if (!ctx->component)
1661                         break;
1662
1663                 switch (ctrl->val) {
1664                 default:
1665                 case V4L2_MPEG_VIDEO_BITRATE_MODE_VBR:
1666                         bitrate_mode = MMAL_VIDEO_RATECONTROL_VARIABLE;
1667                         break;
1668                 case V4L2_MPEG_VIDEO_BITRATE_MODE_CBR:
1669                         bitrate_mode = MMAL_VIDEO_RATECONTROL_CONSTANT;
1670                         break;
1671                 }
1672
1673                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1674                                                     &ctx->component->output[0],
1675                                                     MMAL_PARAMETER_RATECONTROL,
1676                                                     &bitrate_mode,
1677                                                     sizeof(bitrate_mode));
1678                 break;
1679         }
1680         case V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER:
1681                 if (!ctx->component)
1682                         break;
1683
1684                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1685                                                     &ctx->component->output[0],
1686                                                     MMAL_PARAMETER_VIDEO_ENCODE_INLINE_HEADER,
1687                                                     &ctrl->val,
1688                                                     sizeof(ctrl->val));
1689                 break;
1690
1691         case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD:
1692                 if (!ctx->component)
1693                         break;
1694
1695                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1696                                                     &ctx->component->output[0],
1697                                                     MMAL_PARAMETER_INTRAPERIOD,
1698                                                     &ctrl->val,
1699                                                     sizeof(ctrl->val));
1700                 break;
1701
1702         case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
1703         case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
1704                 if (!ctx->component)
1705                         break;
1706
1707                 ret = bcm2835_codec_set_level_profile(ctx, ctrl);
1708                 break;
1709
1710         case V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME: {
1711                 u32 mmal_bool = 1;
1712
1713                 if (!ctx->component)
1714                         break;
1715
1716                 ret = vchiq_mmal_port_parameter_set(ctx->dev->instance,
1717                                                     &ctx->component->output[0],
1718                                                     MMAL_PARAMETER_VIDEO_REQUEST_I_FRAME,
1719                                                     &mmal_bool,
1720                                                     sizeof(mmal_bool));
1721                 break;
1722         }
1723
1724         default:
1725                 v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
1726                 return -EINVAL;
1727         }
1728
1729         if (ret)
1730                 v4l2_err(&ctx->dev->v4l2_dev, "Failed setting ctrl %08x, ret %d\n",
1731                          ctrl->id, ret);
1732         return ret ? -EINVAL : 0;
1733 }
1734
1735 static const struct v4l2_ctrl_ops bcm2835_codec_ctrl_ops = {
1736         .s_ctrl = bcm2835_codec_s_ctrl,
1737 };
1738
1739 static int vidioc_try_decoder_cmd(struct file *file, void *priv,
1740                                   struct v4l2_decoder_cmd *cmd)
1741 {
1742         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1743
1744         if (ctx->dev->role != DECODE)
1745                 return -EINVAL;
1746
1747         switch (cmd->cmd) {
1748         case V4L2_DEC_CMD_STOP:
1749                 if (cmd->flags & V4L2_DEC_CMD_STOP_TO_BLACK) {
1750                         v4l2_err(&ctx->dev->v4l2_dev, "%s: DEC cmd->flags=%u stop to black not supported",
1751                                  __func__, cmd->flags);
1752                         return -EINVAL;
1753                 }
1754                 break;
1755         case V4L2_DEC_CMD_START:
1756                 break;
1757         default:
1758                 return -EINVAL;
1759         }
1760         return 0;
1761 }
1762
1763 static int vidioc_decoder_cmd(struct file *file, void *priv,
1764                               struct v4l2_decoder_cmd *cmd)
1765 {
1766         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1767         struct bcm2835_codec_q_data *q_data = &ctx->q_data[V4L2_M2M_SRC];
1768         int ret;
1769
1770         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s, cmd %u", __func__,
1771                  cmd->cmd);
1772         ret = vidioc_try_decoder_cmd(file, priv, cmd);
1773         if (ret)
1774                 return ret;
1775
1776         switch (cmd->cmd) {
1777         case V4L2_DEC_CMD_STOP:
1778                 if (q_data->eos_buffer_in_use)
1779                         v4l2_err(&ctx->dev->v4l2_dev, "EOS buffers already in use\n");
1780                 q_data->eos_buffer_in_use = true;
1781
1782                 q_data->eos_buffer.mmal.buffer_size = 0;
1783                 q_data->eos_buffer.mmal.length = 0;
1784                 q_data->eos_buffer.mmal.mmal_flags =
1785                                                 MMAL_BUFFER_HEADER_FLAG_EOS;
1786                 q_data->eos_buffer.mmal.pts = 0;
1787                 q_data->eos_buffer.mmal.dts = 0;
1788
1789                 if (!ctx->component)
1790                         break;
1791
1792                 ret = vchiq_mmal_submit_buffer(ctx->dev->instance,
1793                                                &ctx->component->input[0],
1794                                                &q_data->eos_buffer.mmal);
1795                 if (ret)
1796                         v4l2_err(&ctx->dev->v4l2_dev,
1797                                  "%s: EOS buffer submit failed %d\n",
1798                                  __func__, ret);
1799
1800                 break;
1801
1802         case V4L2_DEC_CMD_START:
1803                 /* Do we need to do anything here? */
1804                 break;
1805
1806         default:
1807                 return -EINVAL;
1808         }
1809
1810         return 0;
1811 }
1812
1813 static int vidioc_try_encoder_cmd(struct file *file, void *priv,
1814                                   struct v4l2_encoder_cmd *cmd)
1815 {
1816         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1817
1818         if (ctx->dev->role != ENCODE)
1819                 return -EINVAL;
1820
1821         switch (cmd->cmd) {
1822         case V4L2_ENC_CMD_STOP:
1823                 break;
1824
1825         case V4L2_ENC_CMD_START:
1826                 /* Do we need to do anything here? */
1827                 break;
1828         default:
1829                 return -EINVAL;
1830         }
1831         return 0;
1832 }
1833
1834 static int vidioc_encoder_cmd(struct file *file, void *priv,
1835                               struct v4l2_encoder_cmd *cmd)
1836 {
1837         struct bcm2835_codec_ctx *ctx = file2ctx(file);
1838         struct bcm2835_codec_q_data *q_data = &ctx->q_data[V4L2_M2M_SRC];
1839         int ret;
1840
1841         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s, cmd %u", __func__,
1842                  cmd->cmd);
1843         ret = vidioc_try_encoder_cmd(file, priv, cmd);
1844         if (ret)
1845                 return ret;
1846
1847         switch (cmd->cmd) {
1848         case V4L2_ENC_CMD_STOP:
1849                 if (q_data->eos_buffer_in_use)
1850                         v4l2_err(&ctx->dev->v4l2_dev, "EOS buffers already in use\n");
1851                 q_data->eos_buffer_in_use = true;
1852
1853                 q_data->eos_buffer.mmal.buffer_size = 0;
1854                 q_data->eos_buffer.mmal.length = 0;
1855                 q_data->eos_buffer.mmal.mmal_flags =
1856                                                 MMAL_BUFFER_HEADER_FLAG_EOS;
1857                 q_data->eos_buffer.mmal.pts = 0;
1858                 q_data->eos_buffer.mmal.dts = 0;
1859
1860                 if (!ctx->component)
1861                         break;
1862
1863                 ret = vchiq_mmal_submit_buffer(ctx->dev->instance,
1864                                                &ctx->component->input[0],
1865                                                &q_data->eos_buffer.mmal);
1866                 if (ret)
1867                         v4l2_err(&ctx->dev->v4l2_dev,
1868                                  "%s: EOS buffer submit failed %d\n",
1869                                  __func__, ret);
1870
1871                 break;
1872         case V4L2_ENC_CMD_START:
1873                 /* Do we need to do anything here? */
1874                 break;
1875
1876         default:
1877                 return -EINVAL;
1878         }
1879
1880         return 0;
1881 }
1882
1883 static int vidioc_enum_framesizes(struct file *file, void *fh,
1884                                   struct v4l2_frmsizeenum *fsize)
1885 {
1886         struct bcm2835_codec_fmt *fmt;
1887
1888         fmt = find_format_pix_fmt(fsize->pixel_format, file2ctx(file)->dev,
1889                                   true);
1890         if (!fmt)
1891                 fmt = find_format_pix_fmt(fsize->pixel_format,
1892                                           file2ctx(file)->dev,
1893                                           false);
1894
1895         if (!fmt)
1896                 return -EINVAL;
1897
1898         if (fsize->index)
1899                 return -EINVAL;
1900
1901         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
1902
1903         fsize->stepwise.min_width = MIN_W;
1904         fsize->stepwise.max_width = MAX_W;
1905         fsize->stepwise.step_width = 1;
1906         fsize->stepwise.min_height = MIN_H;
1907         fsize->stepwise.max_height = MAX_H;
1908         fsize->stepwise.step_height = 1;
1909
1910         return 0;
1911 }
1912
1913 static const struct v4l2_ioctl_ops bcm2835_codec_ioctl_ops = {
1914         .vidioc_querycap        = vidioc_querycap,
1915
1916         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
1917         .vidioc_g_fmt_vid_cap_mplane    = vidioc_g_fmt_vid_cap,
1918         .vidioc_try_fmt_vid_cap_mplane  = vidioc_try_fmt_vid_cap,
1919         .vidioc_s_fmt_vid_cap_mplane    = vidioc_s_fmt_vid_cap,
1920
1921         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
1922         .vidioc_g_fmt_vid_out_mplane    = vidioc_g_fmt_vid_out,
1923         .vidioc_try_fmt_vid_out_mplane  = vidioc_try_fmt_vid_out,
1924         .vidioc_s_fmt_vid_out_mplane    = vidioc_s_fmt_vid_out,
1925
1926         .vidioc_reqbufs         = v4l2_m2m_ioctl_reqbufs,
1927         .vidioc_querybuf        = v4l2_m2m_ioctl_querybuf,
1928         .vidioc_qbuf            = v4l2_m2m_ioctl_qbuf,
1929         .vidioc_dqbuf           = v4l2_m2m_ioctl_dqbuf,
1930         .vidioc_prepare_buf     = v4l2_m2m_ioctl_prepare_buf,
1931         .vidioc_create_bufs     = v4l2_m2m_ioctl_create_bufs,
1932         .vidioc_expbuf          = v4l2_m2m_ioctl_expbuf,
1933
1934         .vidioc_streamon        = v4l2_m2m_ioctl_streamon,
1935         .vidioc_streamoff       = v4l2_m2m_ioctl_streamoff,
1936
1937         .vidioc_g_selection     = vidioc_g_selection,
1938         .vidioc_s_selection     = vidioc_s_selection,
1939
1940         .vidioc_g_parm          = vidioc_g_parm,
1941         .vidioc_s_parm          = vidioc_s_parm,
1942
1943         .vidioc_subscribe_event = vidioc_subscribe_evt,
1944         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1945
1946         .vidioc_decoder_cmd = vidioc_decoder_cmd,
1947         .vidioc_try_decoder_cmd = vidioc_try_decoder_cmd,
1948         .vidioc_encoder_cmd = vidioc_encoder_cmd,
1949         .vidioc_try_encoder_cmd = vidioc_try_encoder_cmd,
1950         .vidioc_enum_framesizes = vidioc_enum_framesizes,
1951 };
1952
1953 static int bcm2835_codec_set_ctrls(struct bcm2835_codec_ctx *ctx)
1954 {
1955         /*
1956          * Query the control handler for the value of the various controls and
1957          * set them.
1958          */
1959         const u32 control_ids[] = {
1960                 V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
1961                 V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER,
1962                 V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
1963                 V4L2_CID_MPEG_VIDEO_H264_LEVEL,
1964                 V4L2_CID_MPEG_VIDEO_H264_PROFILE,
1965         };
1966         int i;
1967
1968         for (i = 0; i < ARRAY_SIZE(control_ids); i++) {
1969                 struct v4l2_ctrl *ctrl;
1970
1971                 ctrl = v4l2_ctrl_find(&ctx->hdl, control_ids[i]);
1972                 if (ctrl)
1973                         bcm2835_codec_s_ctrl(ctrl);
1974         }
1975
1976         return 0;
1977 }
1978
1979 static int bcm2835_codec_create_component(struct bcm2835_codec_ctx *ctx)
1980 {
1981         struct bcm2835_codec_dev *dev = ctx->dev;
1982         unsigned int enable = 1;
1983         int ret;
1984
1985         ret = vchiq_mmal_component_init(dev->instance, components[dev->role],
1986                                         &ctx->component);
1987         if (ret < 0) {
1988                 v4l2_err(&dev->v4l2_dev, "%s: failed to create component %s\n",
1989                          __func__, components[dev->role]);
1990                 return -ENOMEM;
1991         }
1992
1993         vchiq_mmal_port_parameter_set(dev->instance, &ctx->component->input[0],
1994                                       MMAL_PARAMETER_ZERO_COPY, &enable,
1995                                       sizeof(enable));
1996         vchiq_mmal_port_parameter_set(dev->instance, &ctx->component->output[0],
1997                                       MMAL_PARAMETER_ZERO_COPY, &enable,
1998                                       sizeof(enable));
1999
2000         setup_mmal_port_format(ctx, &ctx->q_data[V4L2_M2M_SRC],
2001                                &ctx->component->input[0]);
2002
2003         setup_mmal_port_format(ctx, &ctx->q_data[V4L2_M2M_DST],
2004                                &ctx->component->output[0]);
2005
2006         ret = vchiq_mmal_port_set_format(dev->instance,
2007                                          &ctx->component->input[0]);
2008         if (ret < 0) {
2009                 v4l2_dbg(1, debug, &dev->v4l2_dev,
2010                          "%s: vchiq_mmal_port_set_format ip port failed\n",
2011                          __func__);
2012                 goto destroy_component;
2013         }
2014
2015         ret = vchiq_mmal_port_set_format(dev->instance,
2016                                          &ctx->component->output[0]);
2017         if (ret < 0) {
2018                 v4l2_dbg(1, debug, &dev->v4l2_dev,
2019                          "%s: vchiq_mmal_port_set_format op port failed\n",
2020                          __func__);
2021                 goto destroy_component;
2022         }
2023
2024         if (dev->role == ENCODE) {
2025                 u32 param = 1;
2026
2027                 if (ctx->q_data[V4L2_M2M_SRC].sizeimage <
2028                         ctx->component->output[0].minimum_buffer.size)
2029                         v4l2_err(&dev->v4l2_dev, "buffer size mismatch sizeimage %u < min size %u\n",
2030                                  ctx->q_data[V4L2_M2M_SRC].sizeimage,
2031                                  ctx->component->output[0].minimum_buffer.size);
2032
2033                 /* Now we have a component we can set all the ctrls */
2034                 bcm2835_codec_set_ctrls(ctx);
2035
2036                 /* Enable SPS Timing header so framerate information is encoded
2037                  * in the H264 header.
2038                  */
2039                 vchiq_mmal_port_parameter_set(
2040                                         ctx->dev->instance,
2041                                         &ctx->component->output[0],
2042                                         MMAL_PARAMETER_VIDEO_ENCODE_SPS_TIMING,
2043                                         &param, sizeof(param));
2044
2045                 /* Enable inserting headers into the first frame */
2046                 vchiq_mmal_port_parameter_set(ctx->dev->instance,
2047                                               &ctx->component->control,
2048                                               MMAL_PARAMETER_VIDEO_ENCODE_HEADERS_WITH_FRAME,
2049                                               &param, sizeof(param));
2050                 /*
2051                  * Avoid fragmenting the buffers over multiple frames (unless
2052                  * the frame is bigger than the whole buffer)
2053                  */
2054                 vchiq_mmal_port_parameter_set(ctx->dev->instance,
2055                                               &ctx->component->control,
2056                                               MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
2057                                               &param, sizeof(param));
2058         } else {
2059                 if (ctx->q_data[V4L2_M2M_DST].sizeimage <
2060                         ctx->component->output[0].minimum_buffer.size)
2061                         v4l2_err(&dev->v4l2_dev, "buffer size mismatch sizeimage %u < min size %u\n",
2062                                  ctx->q_data[V4L2_M2M_DST].sizeimage,
2063                                  ctx->component->output[0].minimum_buffer.size);
2064         }
2065         v4l2_dbg(2, debug, &dev->v4l2_dev, "%s: component created as %s\n",
2066                  __func__, components[dev->role]);
2067
2068         return 0;
2069
2070 destroy_component:
2071         vchiq_mmal_component_finalise(ctx->dev->instance, ctx->component);
2072         ctx->component = NULL;
2073
2074         return ret;
2075 }
2076
2077 /*
2078  * Queue operations
2079  */
2080
2081 static int bcm2835_codec_queue_setup(struct vb2_queue *vq,
2082                                      unsigned int *nbuffers,
2083                                      unsigned int *nplanes,
2084                                      unsigned int sizes[],
2085                                      struct device *alloc_devs[])
2086 {
2087         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vq);
2088         struct bcm2835_codec_q_data *q_data;
2089         struct vchiq_mmal_port *port;
2090         unsigned int size;
2091
2092         q_data = get_q_data(ctx, vq->type);
2093         if (!q_data)
2094                 return -EINVAL;
2095
2096         if (!ctx->component)
2097                 if (bcm2835_codec_create_component(ctx))
2098                         return -EINVAL;
2099
2100         port = get_port_data(ctx, vq->type);
2101
2102         size = q_data->sizeimage;
2103
2104         if (*nplanes)
2105                 return sizes[0] < size ? -EINVAL : 0;
2106
2107         *nplanes = 1;
2108
2109         sizes[0] = size;
2110         port->current_buffer.size = size;
2111
2112         if (*nbuffers < port->minimum_buffer.num)
2113                 *nbuffers = port->minimum_buffer.num;
2114         /* Add one buffer to take an EOS */
2115         port->current_buffer.num = *nbuffers + 1;
2116
2117         return 0;
2118 }
2119
2120 static int bcm2835_codec_mmal_buf_cleanup(struct mmal_buffer *mmal_buf)
2121 {
2122         mmal_vchi_buffer_cleanup(mmal_buf);
2123
2124         if (mmal_buf->dma_buf) {
2125                 dma_buf_put(mmal_buf->dma_buf);
2126                 mmal_buf->dma_buf = NULL;
2127         }
2128
2129         return 0;
2130 }
2131
2132 static int bcm2835_codec_buf_init(struct vb2_buffer *vb)
2133 {
2134         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
2135         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
2136         struct v4l2_m2m_buffer *m2m = container_of(vb2, struct v4l2_m2m_buffer,
2137                                                    vb);
2138         struct m2m_mmal_buffer *buf = container_of(m2m, struct m2m_mmal_buffer,
2139                                                    m2m);
2140
2141         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: ctx:%p, vb %p\n",
2142                  __func__, ctx, vb);
2143         buf->mmal.buffer = vb2_plane_vaddr(&buf->m2m.vb.vb2_buf, 0);
2144         buf->mmal.buffer_size = vb2_plane_size(&buf->m2m.vb.vb2_buf, 0);
2145
2146         mmal_vchi_buffer_init(ctx->dev->instance, &buf->mmal);
2147
2148         return 0;
2149 }
2150
2151 static int bcm2835_codec_buf_prepare(struct vb2_buffer *vb)
2152 {
2153         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
2154         struct bcm2835_codec_q_data *q_data;
2155         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
2156         struct v4l2_m2m_buffer *m2m = container_of(vbuf, struct v4l2_m2m_buffer,
2157                                                    vb);
2158         struct m2m_mmal_buffer *buf = container_of(m2m, struct m2m_mmal_buffer,
2159                                                    m2m);
2160         struct dma_buf *dma_buf;
2161         int ret;
2162
2163         v4l2_dbg(4, debug, &ctx->dev->v4l2_dev, "%s: type: %d ptr %p\n",
2164                  __func__, vb->vb2_queue->type, vb);
2165
2166         q_data = get_q_data(ctx, vb->vb2_queue->type);
2167         if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
2168                 if (vbuf->field == V4L2_FIELD_ANY)
2169                         vbuf->field = V4L2_FIELD_NONE;
2170                 if (vbuf->field != V4L2_FIELD_NONE) {
2171                         v4l2_err(&ctx->dev->v4l2_dev, "%s field isn't supported\n",
2172                                  __func__);
2173                         return -EINVAL;
2174                 }
2175         }
2176
2177         if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
2178                 v4l2_err(&ctx->dev->v4l2_dev, "%s data will not fit into plane (%lu < %lu)\n",
2179                          __func__, vb2_plane_size(vb, 0),
2180                          (long)q_data->sizeimage);
2181                 return -EINVAL;
2182         }
2183
2184         if (!V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type))
2185                 vb2_set_plane_payload(vb, 0, q_data->sizeimage);
2186
2187         switch (vb->memory) {
2188         case VB2_MEMORY_DMABUF:
2189                 dma_buf = dma_buf_get(vb->planes[0].m.fd);
2190
2191                 if (dma_buf != buf->mmal.dma_buf) {
2192                         /* dmabuf either hasn't already been mapped, or it has
2193                          * changed.
2194                          */
2195                         if (buf->mmal.dma_buf) {
2196                                 v4l2_err(&ctx->dev->v4l2_dev,
2197                                          "%s Buffer changed - why did the core not call cleanup?\n",
2198                                          __func__);
2199                                 bcm2835_codec_mmal_buf_cleanup(&buf->mmal);
2200                         }
2201
2202                         buf->mmal.dma_buf = dma_buf;
2203                 } else {
2204                         /* We already have a reference count on the dmabuf, so
2205                          * release the one we acquired above.
2206                          */
2207                         dma_buf_put(dma_buf);
2208                 }
2209                 ret = 0;
2210                 break;
2211         case VB2_MEMORY_MMAP:
2212                 /*
2213                  * We want to do this at init, but vb2_core_expbuf checks that
2214                  * the index < q->num_buffers, and q->num_buffers only gets
2215                  * updated once all the buffers are allocated.
2216                  */
2217                 if (!buf->mmal.dma_buf) {
2218                         ret = vb2_core_expbuf_dmabuf(vb->vb2_queue,
2219                                                      vb->vb2_queue->type,
2220                                                      vb->index, 0,
2221                                                      O_CLOEXEC,
2222                                                      &buf->mmal.dma_buf);
2223                         if (ret)
2224                                 v4l2_err(&ctx->dev->v4l2_dev,
2225                                          "%s: Failed to expbuf idx %d, ret %d\n",
2226                                          __func__, vb->index, ret);
2227                 } else {
2228                         ret = 0;
2229                 }
2230                 break;
2231         default:
2232                 ret = -EINVAL;
2233                 break;
2234         }
2235
2236         return ret;
2237 }
2238
2239 static void bcm2835_codec_buf_queue(struct vb2_buffer *vb)
2240 {
2241         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
2242         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
2243
2244         v4l2_dbg(4, debug, &ctx->dev->v4l2_dev, "%s: type: %d ptr %p vbuf->flags %u, seq %u, bytesused %u\n",
2245                  __func__, vb->vb2_queue->type, vb, vbuf->flags, vbuf->sequence,
2246                  vb->planes[0].bytesused);
2247         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
2248 }
2249
2250 static void bcm2835_codec_buffer_cleanup(struct vb2_buffer *vb)
2251 {
2252         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
2253         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
2254         struct v4l2_m2m_buffer *m2m = container_of(vb2, struct v4l2_m2m_buffer,
2255                                                    vb);
2256         struct m2m_mmal_buffer *buf = container_of(m2m, struct m2m_mmal_buffer,
2257                                                    m2m);
2258
2259         v4l2_dbg(2, debug, &ctx->dev->v4l2_dev, "%s: ctx:%p, vb %p\n",
2260                  __func__, ctx, vb);
2261
2262         bcm2835_codec_mmal_buf_cleanup(&buf->mmal);
2263 }
2264
2265 static int bcm2835_codec_start_streaming(struct vb2_queue *q,
2266                                          unsigned int count)
2267 {
2268         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(q);
2269         struct bcm2835_codec_dev *dev = ctx->dev;
2270         struct bcm2835_codec_q_data *q_data = get_q_data(ctx, q->type);
2271         int ret;
2272
2273         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: type: %d count %d\n",
2274                  __func__, q->type, count);
2275         q_data->sequence = 0;
2276
2277         if (!ctx->component_enabled) {
2278                 ret = vchiq_mmal_component_enable(dev->instance,
2279                                                   ctx->component);
2280                 if (ret)
2281                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling component, ret %d\n",
2282                                  __func__, ret);
2283                 ctx->component_enabled = true;
2284         }
2285
2286         if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
2287                 /*
2288                  * Create the EOS buffer.
2289                  * We only need the MMAL part, and want to NOT attach a memory
2290                  * buffer to it as it should only take flags.
2291                  */
2292                 memset(&q_data->eos_buffer, 0, sizeof(q_data->eos_buffer));
2293                 mmal_vchi_buffer_init(dev->instance,
2294                                       &q_data->eos_buffer.mmal);
2295                 q_data->eos_buffer_in_use = false;
2296
2297                 ctx->component->input[0].cb_ctx = ctx;
2298                 ret = vchiq_mmal_port_enable(dev->instance,
2299                                              &ctx->component->input[0],
2300                                              ip_buffer_cb);
2301                 if (ret)
2302                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling i/p port, ret %d\n",
2303                                  __func__, ret);
2304         } else {
2305                 ctx->component->output[0].cb_ctx = ctx;
2306                 ret = vchiq_mmal_port_enable(dev->instance,
2307                                              &ctx->component->output[0],
2308                                              op_buffer_cb);
2309                 if (ret)
2310                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling o/p port, ret %d\n",
2311                                  __func__, ret);
2312         }
2313         return ret;
2314 }
2315
2316 static void bcm2835_codec_stop_streaming(struct vb2_queue *q)
2317 {
2318         struct bcm2835_codec_ctx *ctx = vb2_get_drv_priv(q);
2319         struct bcm2835_codec_dev *dev = ctx->dev;
2320         struct bcm2835_codec_q_data *q_data = get_q_data(ctx, q->type);
2321         struct vchiq_mmal_port *port = get_port_data(ctx, q->type);
2322         struct vb2_v4l2_buffer *vbuf;
2323         int ret;
2324
2325         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: type: %d - return buffers\n",
2326                  __func__, q->type);
2327
2328         init_completion(&ctx->frame_cmplt);
2329
2330         /* Clear out all buffers held by m2m framework */
2331         for (;;) {
2332                 if (V4L2_TYPE_IS_OUTPUT(q->type))
2333                         vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
2334                 else
2335                         vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
2336                 if (!vbuf)
2337                         break;
2338                 v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: return buffer %p\n",
2339                          __func__, vbuf);
2340
2341                 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
2342         }
2343
2344         /* Disable MMAL port - this will flush buffers back */
2345         ret = vchiq_mmal_port_disable(dev->instance, port);
2346         if (ret)
2347                 v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed disabling %s port, ret %d\n",
2348                          __func__, V4L2_TYPE_IS_OUTPUT(q->type) ? "i/p" : "o/p",
2349                          ret);
2350
2351         while (atomic_read(&port->buffers_with_vpu)) {
2352                 v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: Waiting for buffers to be returned - %d outstanding\n",
2353                          __func__, atomic_read(&port->buffers_with_vpu));
2354                 ret = wait_for_completion_timeout(&ctx->frame_cmplt, HZ);
2355                 if (ret <= 0) {
2356                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
2357                                  __func__,
2358                                  atomic_read(&port->buffers_with_vpu));
2359                         break;
2360                 }
2361         }
2362
2363
2364         /* If both ports disabled, then disable the component */
2365         if (!ctx->component->input[0].enabled &&
2366             !ctx->component->output[0].enabled) {
2367                 ret = vchiq_mmal_component_disable(dev->instance,
2368                                                    ctx->component);
2369                 if (ret)
2370                         v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed enabling component, ret %d\n",
2371                                  __func__, ret);
2372         }
2373
2374         if (V4L2_TYPE_IS_OUTPUT(q->type))
2375                 mmal_vchi_buffer_cleanup(&q_data->eos_buffer.mmal);
2376
2377         v4l2_dbg(1, debug, &ctx->dev->v4l2_dev, "%s: done\n", __func__);
2378 }
2379
2380 static const struct vb2_ops bcm2835_codec_qops = {
2381         .queue_setup     = bcm2835_codec_queue_setup,
2382         .buf_init        = bcm2835_codec_buf_init,
2383         .buf_prepare     = bcm2835_codec_buf_prepare,
2384         .buf_queue       = bcm2835_codec_buf_queue,
2385         .buf_cleanup     = bcm2835_codec_buffer_cleanup,
2386         .start_streaming = bcm2835_codec_start_streaming,
2387         .stop_streaming  = bcm2835_codec_stop_streaming,
2388         .wait_prepare    = vb2_ops_wait_prepare,
2389         .wait_finish     = vb2_ops_wait_finish,
2390 };
2391
2392 static int queue_init(void *priv, struct vb2_queue *src_vq,
2393                       struct vb2_queue *dst_vq)
2394 {
2395         struct bcm2835_codec_ctx *ctx = priv;
2396         int ret;
2397
2398         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
2399         src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
2400         src_vq->drv_priv = ctx;
2401         src_vq->buf_struct_size = sizeof(struct m2m_mmal_buffer);
2402         src_vq->ops = &bcm2835_codec_qops;
2403         src_vq->mem_ops = &vb2_dma_contig_memops;
2404         src_vq->dev = &ctx->dev->pdev->dev;
2405         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
2406         src_vq->lock = &ctx->dev->dev_mutex;
2407
2408         ret = vb2_queue_init(src_vq);
2409         if (ret)
2410                 return ret;
2411
2412         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
2413         dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
2414         dst_vq->drv_priv = ctx;
2415         dst_vq->buf_struct_size = sizeof(struct m2m_mmal_buffer);
2416         dst_vq->ops = &bcm2835_codec_qops;
2417         dst_vq->mem_ops = &vb2_dma_contig_memops;
2418         dst_vq->dev = &ctx->dev->pdev->dev;
2419         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
2420         dst_vq->lock = &ctx->dev->dev_mutex;
2421
2422         return vb2_queue_init(dst_vq);
2423 }
2424
2425 /*
2426  * File operations
2427  */
2428 static int bcm2835_codec_open(struct file *file)
2429 {
2430         struct bcm2835_codec_dev *dev = video_drvdata(file);
2431         struct bcm2835_codec_ctx *ctx = NULL;
2432         struct v4l2_ctrl_handler *hdl;
2433         int rc = 0;
2434
2435         if (mutex_lock_interruptible(&dev->dev_mutex)) {
2436                 v4l2_err(&dev->v4l2_dev, "Mutex fail\n");
2437                 return -ERESTARTSYS;
2438         }
2439         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
2440         if (!ctx) {
2441                 rc = -ENOMEM;
2442                 goto open_unlock;
2443         }
2444
2445         ctx->q_data[V4L2_M2M_SRC].fmt = get_default_format(dev, false);
2446         ctx->q_data[V4L2_M2M_DST].fmt = get_default_format(dev, true);
2447
2448         ctx->q_data[V4L2_M2M_SRC].crop_width = DEFAULT_WIDTH;
2449         ctx->q_data[V4L2_M2M_SRC].crop_height = DEFAULT_HEIGHT;
2450         ctx->q_data[V4L2_M2M_SRC].height = DEFAULT_HEIGHT;
2451         ctx->q_data[V4L2_M2M_SRC].bytesperline =
2452                         get_bytesperline(DEFAULT_WIDTH,
2453                                          ctx->q_data[V4L2_M2M_SRC].fmt);
2454         ctx->q_data[V4L2_M2M_SRC].sizeimage =
2455                 get_sizeimage(ctx->q_data[V4L2_M2M_SRC].bytesperline,
2456                               ctx->q_data[V4L2_M2M_SRC].crop_width,
2457                               ctx->q_data[V4L2_M2M_SRC].height,
2458                               ctx->q_data[V4L2_M2M_SRC].fmt);
2459
2460         ctx->q_data[V4L2_M2M_DST].crop_width = DEFAULT_WIDTH;
2461         ctx->q_data[V4L2_M2M_DST].crop_height = DEFAULT_HEIGHT;
2462         ctx->q_data[V4L2_M2M_DST].height = DEFAULT_HEIGHT;
2463         ctx->q_data[V4L2_M2M_DST].bytesperline =
2464                         get_bytesperline(DEFAULT_WIDTH,
2465                                          ctx->q_data[V4L2_M2M_DST].fmt);
2466         ctx->q_data[V4L2_M2M_DST].sizeimage =
2467                 get_sizeimage(ctx->q_data[V4L2_M2M_DST].bytesperline,
2468                               ctx->q_data[V4L2_M2M_DST].crop_width,
2469                               ctx->q_data[V4L2_M2M_DST].height,
2470                               ctx->q_data[V4L2_M2M_DST].fmt);
2471
2472         ctx->colorspace = V4L2_COLORSPACE_REC709;
2473         ctx->bitrate = 10 * 1000 * 1000;
2474
2475         ctx->framerate_num = 30;
2476         ctx->framerate_denom = 1;
2477
2478         /* Initialise V4L2 contexts */
2479         v4l2_fh_init(&ctx->fh, video_devdata(file));
2480         file->private_data = &ctx->fh;
2481         ctx->dev = dev;
2482         hdl = &ctx->hdl;
2483         if (dev->role == ENCODE) {
2484                 /* Encode controls */
2485                 v4l2_ctrl_handler_init(hdl, 7);
2486
2487                 v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
2488                                        V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
2489                                        V4L2_MPEG_VIDEO_BITRATE_MODE_CBR, 0,
2490                                        V4L2_MPEG_VIDEO_BITRATE_MODE_VBR);
2491                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
2492                                   V4L2_CID_MPEG_VIDEO_BITRATE,
2493                                   25 * 1000, 25 * 1000 * 1000,
2494                                   25 * 1000, 10 * 1000 * 1000);
2495                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
2496                                   V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER,
2497                                   0, 1,
2498                                   1, 0);
2499                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
2500                                   V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
2501                                   0, 0x7FFFFFFF,
2502                                   1, 60);
2503                 v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
2504                                        V4L2_CID_MPEG_VIDEO_H264_LEVEL,
2505                                        V4L2_MPEG_VIDEO_H264_LEVEL_4_2,
2506                                        ~(BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_0) |
2507                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1B) |
2508                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_1) |
2509                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_2) |
2510                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_3) |
2511                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_0) |
2512                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_1) |
2513                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_2) |
2514                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_0) |
2515                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_1) |
2516                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_2) |
2517                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_0) |
2518                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_1) |
2519                                          BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_2)),
2520                                        V4L2_MPEG_VIDEO_H264_LEVEL_4_0);
2521                 v4l2_ctrl_new_std_menu(hdl, &bcm2835_codec_ctrl_ops,
2522                                        V4L2_CID_MPEG_VIDEO_H264_PROFILE,
2523                                        V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
2524                                        ~(BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
2525                                          BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
2526                                          BIT(V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
2527                                          BIT(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)),
2528                                         V4L2_MPEG_VIDEO_H264_PROFILE_HIGH);
2529                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
2530                                   V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME,
2531                                   0, 0, 0, 0);
2532                 if (hdl->error) {
2533                         rc = hdl->error;
2534                         goto free_ctrl_handler;
2535                 }
2536                 ctx->fh.ctrl_handler = hdl;
2537                 v4l2_ctrl_handler_setup(hdl);
2538         } else if (dev->role == DECODE) {
2539                 v4l2_ctrl_handler_init(hdl, 1);
2540
2541                 v4l2_ctrl_new_std(hdl, &bcm2835_codec_ctrl_ops,
2542                                   V4L2_CID_MIN_BUFFERS_FOR_CAPTURE,
2543                                   1, 1, 1, 1);
2544                 if (hdl->error) {
2545                         rc = hdl->error;
2546                         goto free_ctrl_handler;
2547                 }
2548                 ctx->fh.ctrl_handler = hdl;
2549                 v4l2_ctrl_handler_setup(hdl);
2550         }
2551
2552         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
2553
2554         if (IS_ERR(ctx->fh.m2m_ctx)) {
2555                 rc = PTR_ERR(ctx->fh.m2m_ctx);
2556
2557                 goto free_ctrl_handler;
2558         }
2559
2560         /* Set both queues as buffered as we have buffering in the VPU. That
2561          * means that we will be scheduled whenever either an input or output
2562          * buffer is available (otherwise one of each are required).
2563          */
2564         v4l2_m2m_set_src_buffered(ctx->fh.m2m_ctx, true);
2565         v4l2_m2m_set_dst_buffered(ctx->fh.m2m_ctx, true);
2566
2567         v4l2_fh_add(&ctx->fh);
2568         atomic_inc(&dev->num_inst);
2569
2570         mutex_unlock(&dev->dev_mutex);
2571         return 0;
2572
2573 free_ctrl_handler:
2574         v4l2_ctrl_handler_free(hdl);
2575         kfree(ctx);
2576 open_unlock:
2577         mutex_unlock(&dev->dev_mutex);
2578         return rc;
2579 }
2580
2581 static int bcm2835_codec_release(struct file *file)
2582 {
2583         struct bcm2835_codec_dev *dev = video_drvdata(file);
2584         struct bcm2835_codec_ctx *ctx = file2ctx(file);
2585
2586         v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: Releasing instance %p\n",
2587                  __func__, ctx);
2588
2589         v4l2_fh_del(&ctx->fh);
2590         v4l2_fh_exit(&ctx->fh);
2591         v4l2_ctrl_handler_free(&ctx->hdl);
2592         mutex_lock(&dev->dev_mutex);
2593         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
2594
2595         if (ctx->component)
2596                 vchiq_mmal_component_finalise(dev->instance, ctx->component);
2597
2598         mutex_unlock(&dev->dev_mutex);
2599         kfree(ctx);
2600
2601         atomic_dec(&dev->num_inst);
2602
2603         return 0;
2604 }
2605
2606 static const struct v4l2_file_operations bcm2835_codec_fops = {
2607         .owner          = THIS_MODULE,
2608         .open           = bcm2835_codec_open,
2609         .release        = bcm2835_codec_release,
2610         .poll           = v4l2_m2m_fop_poll,
2611         .unlocked_ioctl = video_ioctl2,
2612         .mmap           = v4l2_m2m_fop_mmap,
2613 };
2614
2615 static const struct video_device bcm2835_codec_videodev = {
2616         .name           = MEM2MEM_NAME,
2617         .vfl_dir        = VFL_DIR_M2M,
2618         .fops           = &bcm2835_codec_fops,
2619         .ioctl_ops      = &bcm2835_codec_ioctl_ops,
2620         .minor          = -1,
2621         .release        = video_device_release_empty,
2622 };
2623
2624 static const struct v4l2_m2m_ops m2m_ops = {
2625         .device_run     = device_run,
2626         .job_ready      = job_ready,
2627         .job_abort      = job_abort,
2628 };
2629
2630 /* Size of the array to provide to the VPU when asking for the list of supported
2631  * formats.
2632  * The ISP component currently advertises 44 input formats, so add a small
2633  * overhead on that.
2634  */
2635 #define MAX_SUPPORTED_ENCODINGS 50
2636
2637 /* Populate dev->supported_fmts with the formats supported by those ports. */
2638 static int bcm2835_codec_get_supported_fmts(struct bcm2835_codec_dev *dev)
2639 {
2640         struct bcm2835_codec_fmt *list;
2641         struct vchiq_mmal_component *component;
2642         u32 fourccs[MAX_SUPPORTED_ENCODINGS];
2643         u32 param_size = sizeof(fourccs);
2644         unsigned int i, j, num_encodings;
2645         int ret;
2646
2647         ret = vchiq_mmal_component_init(dev->instance, components[dev->role],
2648                                         &component);
2649         if (ret < 0) {
2650                 v4l2_err(&dev->v4l2_dev, "%s: failed to create component %s\n",
2651                          __func__, components[dev->role]);
2652                 return -ENOMEM;
2653         }
2654
2655         ret = vchiq_mmal_port_parameter_get(dev->instance,
2656                                             &component->input[0],
2657                                             MMAL_PARAMETER_SUPPORTED_ENCODINGS,
2658                                             &fourccs,
2659                                             &param_size);
2660
2661         if (ret) {
2662                 if (ret == MMAL_MSG_STATUS_ENOSPC) {
2663                         v4l2_err(&dev->v4l2_dev, "%s: port has more encoding than we provided space for. Some are dropped.\n",
2664                                  __func__);
2665                         num_encodings = MAX_SUPPORTED_ENCODINGS;
2666                 } else {
2667                         v4l2_err(&dev->v4l2_dev, "%s: get_param ret %u.\n",
2668                                  __func__, ret);
2669                         ret = -EINVAL;
2670                         goto destroy_component;
2671                 }
2672         } else {
2673                 num_encodings = param_size / sizeof(u32);
2674         }
2675
2676         /* Assume at this stage that all encodings will be supported in V4L2.
2677          * Any that aren't supported will waste a very small amount of memory.
2678          */
2679         list = devm_kzalloc(&dev->pdev->dev,
2680                             sizeof(struct bcm2835_codec_fmt) * num_encodings,
2681                             GFP_KERNEL);
2682         if (!list) {
2683                 ret = -ENOMEM;
2684                 goto destroy_component;
2685         }
2686         dev->supported_fmts[0].list = list;
2687
2688         for (i = 0, j = 0; i < num_encodings; i++) {
2689                 const struct bcm2835_codec_fmt *fmt = get_fmt(fourccs[i]);
2690
2691                 if (fmt) {
2692                         list[j] = *fmt;
2693                         j++;
2694                 }
2695         }
2696         dev->supported_fmts[0].num_entries = j;
2697
2698         param_size = sizeof(fourccs);
2699         ret = vchiq_mmal_port_parameter_get(dev->instance,
2700                                             &component->output[0],
2701                                             MMAL_PARAMETER_SUPPORTED_ENCODINGS,
2702                                             &fourccs,
2703                                             &param_size);
2704
2705         if (ret) {
2706                 if (ret == MMAL_MSG_STATUS_ENOSPC) {
2707                         v4l2_err(&dev->v4l2_dev, "%s: port has more encoding than we provided space for. Some are dropped.\n",
2708                                  __func__);
2709                         num_encodings = MAX_SUPPORTED_ENCODINGS;
2710                 } else {
2711                         ret = -EINVAL;
2712                         goto destroy_component;
2713                 }
2714         } else {
2715                 num_encodings = param_size / sizeof(u32);
2716         }
2717         /* Assume at this stage that all encodings will be supported in V4L2. */
2718         list = devm_kzalloc(&dev->pdev->dev,
2719                             sizeof(struct bcm2835_codec_fmt) * num_encodings,
2720                             GFP_KERNEL);
2721         if (!list) {
2722                 ret = -ENOMEM;
2723                 goto destroy_component;
2724         }
2725         dev->supported_fmts[1].list = list;
2726
2727         for (i = 0, j = 0; i < num_encodings; i++) {
2728                 const struct bcm2835_codec_fmt *fmt = get_fmt(fourccs[i]);
2729
2730                 if (fmt) {
2731                         list[j] = *fmt;
2732                         j++;
2733                 }
2734         }
2735         dev->supported_fmts[1].num_entries = j;
2736
2737         ret = 0;
2738
2739 destroy_component:
2740         vchiq_mmal_component_finalise(dev->instance, component);
2741
2742         return ret;
2743 }
2744
2745 static int bcm2835_codec_create(struct bcm2835_codec_driver *drv,
2746                                 struct bcm2835_codec_dev **new_dev,
2747                                 enum bcm2835_codec_role role)
2748 {
2749         struct platform_device *pdev = drv->pdev;
2750         struct bcm2835_codec_dev *dev;
2751         struct video_device *vfd;
2752         int function;
2753         int video_nr;
2754         int ret;
2755
2756         dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
2757         if (!dev)
2758                 return -ENOMEM;
2759
2760         dev->pdev = pdev;
2761
2762         dev->role = role;
2763
2764         ret = vchiq_mmal_init(&dev->instance);
2765         if (ret)
2766                 return ret;
2767
2768         ret = bcm2835_codec_get_supported_fmts(dev);
2769         if (ret)
2770                 goto vchiq_finalise;
2771
2772         atomic_set(&dev->num_inst, 0);
2773         mutex_init(&dev->dev_mutex);
2774
2775         /* Initialise the video device */
2776         dev->vfd = bcm2835_codec_videodev;
2777
2778         vfd = &dev->vfd;
2779         vfd->lock = &dev->dev_mutex;
2780         vfd->v4l2_dev = &dev->v4l2_dev;
2781         vfd->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
2782         vfd->v4l2_dev->mdev = &drv->mdev;
2783
2784         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
2785         if (ret)
2786                 goto vchiq_finalise;
2787
2788         switch (role) {
2789         case DECODE:
2790                 v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
2791                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
2792                 v4l2_disable_ioctl(vfd, VIDIOC_S_PARM);
2793                 v4l2_disable_ioctl(vfd, VIDIOC_G_PARM);
2794                 function = MEDIA_ENT_F_PROC_VIDEO_DECODER;
2795                 video_nr = decode_video_nr;
2796                 break;
2797         case ENCODE:
2798                 v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
2799                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
2800                 function = MEDIA_ENT_F_PROC_VIDEO_ENCODER;
2801                 video_nr = encode_video_nr;
2802                 break;
2803         case ISP:
2804                 v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
2805                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
2806                 v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
2807                 v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
2808                 v4l2_disable_ioctl(vfd, VIDIOC_S_PARM);
2809                 v4l2_disable_ioctl(vfd, VIDIOC_G_PARM);
2810                 function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
2811                 video_nr = isp_video_nr;
2812                 break;
2813         default:
2814                 ret = -EINVAL;
2815                 goto unreg_dev;
2816         }
2817
2818         ret = video_register_device(vfd, VFL_TYPE_GRABBER, video_nr);
2819         if (ret) {
2820                 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
2821                 goto unreg_dev;
2822         }
2823
2824         video_set_drvdata(vfd, dev);
2825         snprintf(vfd->name, sizeof(vfd->name), "%s-%s",
2826                  bcm2835_codec_videodev.name, roles[role]);
2827         v4l2_info(&dev->v4l2_dev, "Device registered as /dev/video%d\n",
2828                   vfd->num);
2829
2830         *new_dev = dev;
2831
2832         dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
2833         if (IS_ERR(dev->m2m_dev)) {
2834                 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
2835                 ret = PTR_ERR(dev->m2m_dev);
2836                 goto err_m2m;
2837         }
2838
2839         ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd, function);
2840         if (ret)
2841                 goto err_m2m;
2842
2843         v4l2_info(&dev->v4l2_dev, "Loaded V4L2 %s\n",
2844                   roles[role]);
2845         return 0;
2846
2847 err_m2m:
2848         v4l2_m2m_release(dev->m2m_dev);
2849         video_unregister_device(&dev->vfd);
2850 unreg_dev:
2851         v4l2_device_unregister(&dev->v4l2_dev);
2852 vchiq_finalise:
2853         vchiq_mmal_finalise(dev->instance);
2854         return ret;
2855 }
2856
2857 static int bcm2835_codec_destroy(struct bcm2835_codec_dev *dev)
2858 {
2859         if (!dev)
2860                 return -ENODEV;
2861
2862         v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME ", %s\n",
2863                   roles[dev->role]);
2864         v4l2_m2m_unregister_media_controller(dev->m2m_dev);
2865         v4l2_m2m_release(dev->m2m_dev);
2866         video_unregister_device(&dev->vfd);
2867         v4l2_device_unregister(&dev->v4l2_dev);
2868         vchiq_mmal_finalise(dev->instance);
2869
2870         return 0;
2871 }
2872
2873 static int bcm2835_codec_probe(struct platform_device *pdev)
2874 {
2875         struct bcm2835_codec_driver *drv;
2876         struct media_device *mdev;
2877         int ret = 0;
2878
2879         drv = devm_kzalloc(&pdev->dev, sizeof(*drv), GFP_KERNEL);
2880         if (!drv)
2881                 return -ENOMEM;
2882
2883         drv->pdev = pdev;
2884         mdev = &drv->mdev;
2885         mdev->dev = &pdev->dev;
2886
2887         strscpy(mdev->model, bcm2835_codec_videodev.name, sizeof(mdev->model));
2888         strscpy(mdev->serial, "0000", sizeof(mdev->serial));
2889         snprintf(mdev->bus_info, sizeof(mdev->bus_info), "platform:%s",
2890                  pdev->name);
2891
2892         /* This should return the vgencmd version information or such .. */
2893         mdev->hw_revision = 1;
2894         media_device_init(mdev);
2895
2896         ret = bcm2835_codec_create(drv, &drv->decode, DECODE);
2897         if (ret)
2898                 goto out;
2899
2900         ret = bcm2835_codec_create(drv, &drv->encode, ENCODE);
2901         if (ret)
2902                 goto out;
2903
2904         ret = bcm2835_codec_create(drv, &drv->isp, ISP);
2905         if (ret)
2906                 goto out;
2907
2908         /* Register the media device node */
2909         if (media_device_register(mdev) < 0)
2910                 goto out;
2911
2912         platform_set_drvdata(pdev, drv);
2913
2914         return 0;
2915
2916 out:
2917         if (drv->isp) {
2918                 bcm2835_codec_destroy(drv->isp);
2919                 drv->isp = NULL;
2920         }
2921         if (drv->encode) {
2922                 bcm2835_codec_destroy(drv->encode);
2923                 drv->encode = NULL;
2924         }
2925         if (drv->decode) {
2926                 bcm2835_codec_destroy(drv->decode);
2927                 drv->decode = NULL;
2928         }
2929         return ret;
2930 }
2931
2932 static int bcm2835_codec_remove(struct platform_device *pdev)
2933 {
2934         struct bcm2835_codec_driver *drv = platform_get_drvdata(pdev);
2935
2936         media_device_unregister(&drv->mdev);
2937
2938         bcm2835_codec_destroy(drv->isp);
2939
2940         bcm2835_codec_destroy(drv->encode);
2941
2942         bcm2835_codec_destroy(drv->decode);
2943
2944         media_device_cleanup(&drv->mdev);
2945
2946         return 0;
2947 }
2948
2949 static struct platform_driver bcm2835_v4l2_codec_driver = {
2950         .probe = bcm2835_codec_probe,
2951         .remove = bcm2835_codec_remove,
2952         .driver = {
2953                    .name = "bcm2835-codec",
2954                    .owner = THIS_MODULE,
2955                    },
2956 };
2957
2958 module_platform_driver(bcm2835_v4l2_codec_driver);
2959
2960 MODULE_DESCRIPTION("BCM2835 codec V4L2 driver");
2961 MODULE_AUTHOR("Dave Stevenson, <dave.stevenson@raspberrypi.org>");
2962 MODULE_LICENSE("GPL");
2963 MODULE_VERSION("0.0.1");
2964 MODULE_ALIAS("platform:bcm2835-codec");