drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Broadcom BM2835 V4L2 driver
   4  *
   5  * Copyright © 2013 Raspberry Pi (Trading) Ltd.
   6  *
   7  * Authors: Vincent Sanders @ Collabora
   8  *          Dave Stevenson @ Broadcom
   9  *              (now dave.stevenson@raspberrypi.org)
  10  *          Simon Mellor @ Broadcom
  11  *          Luke Diamand @ Broadcom
  12  */
  13
  14 #include <linux/errno.h>
  15 #include <linux/kernel.h>
  16 #include <linux/module.h>
  17 #include <linux/slab.h>
  18 #include <media/videobuf2-vmalloc.h>
  19 #include <media/videobuf2-dma-contig.h>
  20 #include <media/v4l2-device.h>
  21 #include <media/v4l2-ioctl.h>
  22 #include <media/v4l2-ctrls.h>
  23 #include <media/v4l2-fh.h>
  24 #include <media/v4l2-event.h>
  25 #include <media/v4l2-common.h>
  26 #include <linux/delay.h>
  27 #include <linux/platform_device.h>
  28
  29 #include "mmal-common.h"
  30 #include "mmal-encodings.h"
  31 #include "mmal-vchiq.h"
  32 #include "mmal-msg.h"
  33 #include "mmal-parameters.h"
  34 #include "bcm2835-camera.h"
  35
  36 #define BM2835_MMAL_VERSION "0.0.2"
  37 #define BM2835_MMAL_MODULE_NAME "bcm2835-v4l2"
  38 #define MIN_WIDTH 32
  39 #define MIN_HEIGHT 32
  40 #define MIN_BUFFER_SIZE (80 * 1024)
  41
  42 #define MAX_VIDEO_MODE_WIDTH 1280
  43 #define MAX_VIDEO_MODE_HEIGHT 720
  44
  45 #define MAX_BCM2835_CAMERAS 2
  46
  47 int bcm2835_v4l2_debug;
  48 module_param_named(debug, bcm2835_v4l2_debug, int, 0644);
  49 MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2");
  50
  51 #define UNSET (-1)
  52 static int video_nr[] = {[0 ... (MAX_BCM2835_CAMERAS - 1)] = UNSET };
  53 module_param_array(video_nr, int, NULL, 0644);
  54 MODULE_PARM_DESC(video_nr, "videoX start numbers, -1 is autodetect");
  55
  56 static int max_video_width = MAX_VIDEO_MODE_WIDTH;
  57 static int max_video_height = MAX_VIDEO_MODE_HEIGHT;
  58 module_param(max_video_width, int, 0644);
  59 MODULE_PARM_DESC(max_video_width, "Threshold for video mode");
  60 module_param(max_video_height, int, 0644);
  61 MODULE_PARM_DESC(max_video_height, "Threshold for video mode");
  62
  63 /* global device data array */
  64 static struct bm2835_mmal_dev *gdev[MAX_BCM2835_CAMERAS];
  65
  66 #define FPS_MIN 1
  67 #define FPS_MAX 90
  68
  69 /* timeperframe: min/max and default */
  70 static const struct v4l2_fract
  71         tpf_min     = {.numerator = 1,          .denominator = FPS_MAX},
  72         tpf_max     = {.numerator = 1,          .denominator = FPS_MIN},
  73         tpf_default = {.numerator = 1000,       .denominator = 30000};
  74
  75 /* Container for MMAL and VB2 buffers*/
  76 struct vb2_mmal_buffer {
  77         struct vb2_v4l2_buffer  vb;
  78         struct mmal_buffer      mmal;
  79 };
  80
  81 /* video formats */
  82 static struct mmal_fmt formats[] = {
  83         {
  84                 .name = "4:2:0, planar, YUV",
  85                 .fourcc = V4L2_PIX_FMT_YUV420,
  86                 .flags = 0,
  87                 .mmal = MMAL_ENCODING_I420,
  88                 .depth = 12,
  89                 .mmal_component = COMP_CAMERA,
  90                 .ybbp = 1,
  91                 .remove_padding = 1,
  92         }, {
  93                 .name = "4:2:2, packed, YUYV",
  94                 .fourcc = V4L2_PIX_FMT_YUYV,
  95                 .flags = 0,
  96                 .mmal = MMAL_ENCODING_YUYV,
  97                 .depth = 16,
  98                 .mmal_component = COMP_CAMERA,
  99                 .ybbp = 2,
 100                 .remove_padding = 0,
 101         }, {
 102                 .name = "RGB24 (LE)",
 103                 .fourcc = V4L2_PIX_FMT_RGB24,
 104                 .flags = 0,
 105                 .mmal = MMAL_ENCODING_RGB24,
 106                 .depth = 24,
 107                 .mmal_component = COMP_CAMERA,
 108                 .ybbp = 3,
 109                 .remove_padding = 0,
 110         }, {
 111                 .name = "JPEG",
 112                 .fourcc = V4L2_PIX_FMT_JPEG,
 113                 .flags = V4L2_FMT_FLAG_COMPRESSED,
 114                 .mmal = MMAL_ENCODING_JPEG,
 115                 .depth = 8,
 116                 .mmal_component = COMP_IMAGE_ENCODE,
 117                 .ybbp = 0,
 118                 .remove_padding = 0,
 119         }, {
 120                 .name = "H264",
 121                 .fourcc = V4L2_PIX_FMT_H264,
 122                 .flags = V4L2_FMT_FLAG_COMPRESSED,
 123                 .mmal = MMAL_ENCODING_H264,
 124                 .depth = 8,
 125                 .mmal_component = COMP_VIDEO_ENCODE,
 126                 .ybbp = 0,
 127                 .remove_padding = 0,
 128         }, {
 129                 .name = "MJPEG",
 130                 .fourcc = V4L2_PIX_FMT_MJPEG,
 131                 .flags = V4L2_FMT_FLAG_COMPRESSED,
 132                 .mmal = MMAL_ENCODING_MJPEG,
 133                 .depth = 8,
 134                 .mmal_component = COMP_VIDEO_ENCODE,
 135                 .ybbp = 0,
 136                 .remove_padding = 0,
 137         }, {
 138                 .name = "4:2:2, packed, YVYU",
 139                 .fourcc = V4L2_PIX_FMT_YVYU,
 140                 .flags = 0,
 141                 .mmal = MMAL_ENCODING_YVYU,
 142                 .depth = 16,
 143                 .mmal_component = COMP_CAMERA,
 144                 .ybbp = 2,
 145                 .remove_padding = 0,
 146         }, {
 147                 .name = "4:2:2, packed, VYUY",
 148                 .fourcc = V4L2_PIX_FMT_VYUY,
 149                 .flags = 0,
 150                 .mmal = MMAL_ENCODING_VYUY,
 151                 .depth = 16,
 152                 .mmal_component = COMP_CAMERA,
 153                 .ybbp = 2,
 154                 .remove_padding = 0,
 155         }, {
 156                 .name = "4:2:2, packed, UYVY",
 157                 .fourcc = V4L2_PIX_FMT_UYVY,
 158                 .flags = 0,
 159                 .mmal = MMAL_ENCODING_UYVY,
 160                 .depth = 16,
 161                 .mmal_component = COMP_CAMERA,
 162                 .ybbp = 2,
 163                 .remove_padding = 0,
 164         }, {
 165                 .name = "4:2:0, planar, NV12",
 166                 .fourcc = V4L2_PIX_FMT_NV12,
 167                 .flags = 0,
 168                 .mmal = MMAL_ENCODING_NV12,
 169                 .depth = 12,
 170                 .mmal_component = COMP_CAMERA,
 171                 .ybbp = 1,
 172                 .remove_padding = 1,
 173         }, {
 174                 .name = "RGB24 (BE)",
 175                 .fourcc = V4L2_PIX_FMT_BGR24,
 176                 .flags = 0,
 177                 .mmal = MMAL_ENCODING_BGR24,
 178                 .depth = 24,
 179                 .mmal_component = COMP_CAMERA,
 180                 .ybbp = 3,
 181                 .remove_padding = 0,
 182         }, {
 183                 .name = "4:2:0, planar, YVU",
 184                 .fourcc = V4L2_PIX_FMT_YVU420,
 185                 .flags = 0,
 186                 .mmal = MMAL_ENCODING_YV12,
 187                 .depth = 12,
 188                 .mmal_component = COMP_CAMERA,
 189                 .ybbp = 1,
 190                 .remove_padding = 1,
 191         }, {
 192                 .name = "4:2:0, planar, NV21",
 193                 .fourcc = V4L2_PIX_FMT_NV21,
 194                 .flags = 0,
 195                 .mmal = MMAL_ENCODING_NV21,
 196                 .depth = 12,
 197                 .mmal_component = COMP_CAMERA,
 198                 .ybbp = 1,
 199                 .remove_padding = 1,
 200         }, {
 201                 .name = "RGB32 (BE)",
 202                 .fourcc = V4L2_PIX_FMT_BGR32,
 203                 .flags = 0,
 204                 .mmal = MMAL_ENCODING_BGRA,
 205                 .depth = 32,
 206                 .mmal_component = COMP_CAMERA,
 207                 .ybbp = 4,
 208                 .remove_padding = 0,
 209         },
 210 };
 211
 212 static struct mmal_fmt *get_format(struct v4l2_format *f)
 213 {
 214         struct mmal_fmt *fmt;
 215         unsigned int k;
 216
 217         for (k = 0; k < ARRAY_SIZE(formats); k++) {
 218                 fmt = &formats[k];
 219                 if (fmt->fourcc == f->fmt.pix.pixelformat)
 220                         return fmt;
 221         }
 222
 223         return NULL;
 224 }
 225
 226 /* ------------------------------------------------------------------
 227  *      Videobuf queue operations
 228  * ------------------------------------------------------------------
 229  */
 230
 231 static int queue_setup(struct vb2_queue *vq,
 232                        unsigned int *nbuffers, unsigned int *nplanes,
 233                        unsigned int sizes[], struct device *alloc_ctxs[])
 234 {
 235         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
 236         unsigned long size;
 237
 238         /* refuse queue setup if port is not configured */
 239         if (!dev->capture.port) {
 240                 v4l2_err(&dev->v4l2_dev,
 241                          "%s: capture port not configured\n", __func__);
 242                 return -EINVAL;
 243         }
 244
 245         /* Handle CREATE_BUFS situation - *nplanes != 0 */
 246         if (*nplanes) {
 247                 if (*nplanes != 1 ||
 248                     sizes[0] < dev->capture.port->current_buffer.size) {
 249                         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 250                                  "%s: dev:%p Invalid buffer request from CREATE_BUFS, size %u < %u, nplanes %u != 1\n",
 251                                  __func__, dev, sizes[0],
 252                                  dev->capture.port->current_buffer.size,
 253                                  *nplanes);
 254                         return -EINVAL;
 255                 } else {
 256                         return 0;
 257                 }
 258         }
 259
 260         /* Handle REQBUFS situation */
 261         size = dev->capture.port->current_buffer.size;
 262         if (size == 0) {
 263                 v4l2_err(&dev->v4l2_dev,
 264                          "%s: capture port buffer size is zero\n", __func__);
 265                 return -EINVAL;
 266         }
 267
 268         if (*nbuffers < dev->capture.port->minimum_buffer.num)
 269                 *nbuffers = dev->capture.port->minimum_buffer.num;
 270
 271         dev->capture.port->current_buffer.num = *nbuffers;
 272
 273         *nplanes = 1;
 274
 275         sizes[0] = size;
 276
 277         /*
 278          * videobuf2-vmalloc allocator is context-less so no need to set
 279          * alloc_ctxs array.
 280          */
 281
 282         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
 283                  __func__, dev);
 284
 285         return 0;
 286 }
 287
 288 static int buffer_init(struct vb2_buffer *vb)
 289 {
 290         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 291         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 292         struct vb2_mmal_buffer *buf =
 293                                 container_of(vb2, struct vb2_mmal_buffer, vb);
 294
 295         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
 296                  __func__, dev, vb);
 297         buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
 298         buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
 299
 300         return mmal_vchi_buffer_init(dev->instance, &buf->mmal);
 301 }
 302
 303 static int buffer_prepare(struct vb2_buffer *vb)
 304 {
 305         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 306         unsigned long size;
 307
 308         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
 309                  __func__, dev, vb);
 310
 311         if (!dev->capture.port || !dev->capture.fmt)
 312                 return -ENODEV;
 313
 314         size = dev->capture.stride * dev->capture.height;
 315         if (vb2_plane_size(vb, 0) < size) {
 316                 v4l2_err(&dev->v4l2_dev,
 317                          "%s data will not fit into plane (%lu < %lu)\n",
 318                          __func__, vb2_plane_size(vb, 0), size);
 319                 return -EINVAL;
 320         }
 321
 322         return 0;
 323 }
 324
 325 static void buffer_cleanup(struct vb2_buffer *vb)
 326 {
 327         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 328         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 329         struct vb2_mmal_buffer *buf =
 330                                 container_of(vb2, struct vb2_mmal_buffer, vb);
 331
 332         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
 333                  __func__, dev, vb);
 334
 335         mmal_vchi_buffer_cleanup(&buf->mmal);
 336 }
 337
 338 static inline bool is_capturing(struct bm2835_mmal_dev *dev)
 339 {
 340         return dev->capture.camera_port ==
 341             &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
 342 }
 343
 344 static void buffer_cb(struct vchiq_mmal_instance *instance,
 345                       struct vchiq_mmal_port *port,
 346                       int status,
 347                       struct mmal_buffer *mmal_buf)
 348 {
 349         struct bm2835_mmal_dev *dev = port->cb_ctx;
 350         struct vb2_mmal_buffer *buf =
 351                         container_of(mmal_buf, struct vb2_mmal_buffer, mmal);
 352
 353         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 354                  "%s: status:%d, buf:%p, length:%lu, flags %u, pts %lld\n",
 355                  __func__, status, buf, mmal_buf->length, mmal_buf->mmal_flags,
 356                  mmal_buf->pts);
 357
 358         if (status != 0) {
 359                 /* error in transfer */
 360                 if (buf) {
 361                         /* there was a buffer with the error so return it */
 362                         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 363                 }
 364                 return;
 365         }
 366
 367         if (mmal_buf->length == 0) {
 368                 /* stream ended */
 369                 if (dev->capture.frame_count) {
 370                         /* empty buffer whilst capturing - expected to be an
 371                          * EOS, so grab another frame
 372                          */
 373                         if (is_capturing(dev)) {
 374                                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 375                                          "Grab another frame");
 376                                 vchiq_mmal_port_parameter_set(
 377                                         instance,
 378                                         dev->capture.camera_port,
 379                                         MMAL_PARAMETER_CAPTURE,
 380                                         &dev->capture.frame_count,
 381                                         sizeof(dev->capture.frame_count));
 382                         }
 383                         if (vchiq_mmal_submit_buffer(instance, port,
 384                                                      &buf->mmal))
 385                                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 386                                          "Failed to return EOS buffer");
 387                 } else {
 388                         /* stopping streaming.
 389                          * return buffer, and signal frame completion
 390                          */
 391                         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 392                         complete(&dev->capture.frame_cmplt);
 393                 }
 394                 return;
 395         }
 396
 397         if (!dev->capture.frame_count) {
 398                 /* signal frame completion */
 399                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 400                 complete(&dev->capture.frame_cmplt);
 401                 return;
 402         }
 403
 404         if (dev->capture.vc_start_timestamp == -1) {
 405                 /*
 406                  * VPU doesn't support MMAL_PARAMETER_SYSTEM_TIME, rely on
 407                  * kernel time, and have no latency compensation.
 408                  */
 409                 buf->vb.vb2_buf.timestamp = ktime_get_ns();
 410                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 411                          "Buffer time set as current time - %lld",
 412                          buf->vb.vb2_buf.timestamp);
 413         } else if (mmal_buf->pts != 0) {
 414                 ktime_t timestamp;
 415                 s64 runtime_us = mmal_buf->pts -
 416                     dev->capture.vc_start_timestamp;
 417                 timestamp = ktime_add_us(dev->capture.kernel_start_ts,
 418                                          runtime_us);
 419                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 420                          "Convert start time %llu and %llu with offset %llu to %llu\n",
 421                          ktime_to_ns(dev->capture.kernel_start_ts),
 422                          dev->capture.vc_start_timestamp, mmal_buf->pts,
 423                          ktime_to_ns(timestamp));
 424                 if (timestamp < dev->capture.last_timestamp) {
 425                         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 426                                  "Negative delta - using last time\n");
 427                         timestamp = dev->capture.last_timestamp;
 428                 }
 429                 buf->vb.vb2_buf.timestamp = ktime_to_ns(timestamp);
 430         } else {
 431                 if (dev->capture.last_timestamp) {
 432                         buf->vb.vb2_buf.timestamp = dev->capture.last_timestamp;
 433                         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 434                                  "Buffer time set as last timestamp - %lld",
 435                                  buf->vb.vb2_buf.timestamp);
 436                 } else {
 437                         buf->vb.vb2_buf.timestamp =
 438                                 ktime_to_ns(dev->capture.kernel_start_ts);
 439                         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 440                                  "Buffer time set as start timestamp - %lld",
 441                                  buf->vb.vb2_buf.timestamp);
 442                 }
 443         }
 444         dev->capture.last_timestamp = buf->vb.vb2_buf.timestamp;
 445         buf->vb.sequence = dev->capture.sequence++;
 446         buf->vb.field = V4L2_FIELD_NONE;
 447
 448         vb2_set_plane_payload(&buf->vb.vb2_buf, 0, mmal_buf->length);
 449         if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
 450                 buf->vb.flags |= V4L2_BUF_FLAG_KEYFRAME;
 451
 452         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 453                  "Buffer has ts %llu", dev->capture.last_timestamp);
 454         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 455
 456         if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS &&
 457             is_capturing(dev)) {
 458                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 459                          "Grab another frame as buffer has EOS");
 460                 vchiq_mmal_port_parameter_set(instance,
 461                                               dev->capture.camera_port,
 462                                               MMAL_PARAMETER_CAPTURE,
 463                                               &dev->capture.frame_count,
 464                                               sizeof(dev->capture.frame_count));
 465         }
 466 }
 467
 468 static int enable_camera(struct bm2835_mmal_dev *dev)
 469 {
 470         int ret;
 471
 472         if (!dev->camera_use_count) {
 473                 ret = vchiq_mmal_port_parameter_set(
 474                         dev->instance,
 475                         &dev->component[COMP_CAMERA]->control,
 476                         MMAL_PARAMETER_CAMERA_NUM, &dev->camera_num,
 477                         sizeof(dev->camera_num));
 478                 if (ret < 0) {
 479                         v4l2_err(&dev->v4l2_dev,
 480                                  "Failed setting camera num, ret %d\n", ret);
 481                         return -EINVAL;
 482                 }
 483
 484                 ret = vchiq_mmal_component_enable(
 485                                 dev->instance,
 486                                 dev->component[COMP_CAMERA]);
 487                 if (ret < 0) {
 488                         v4l2_err(&dev->v4l2_dev,
 489                                  "Failed enabling camera, ret %d\n", ret);
 490                         return -EINVAL;
 491                 }
 492         }
 493         dev->camera_use_count++;
 494         v4l2_dbg(1, bcm2835_v4l2_debug,
 495                  &dev->v4l2_dev, "enabled camera (refcount %d)\n",
 496                         dev->camera_use_count);
 497         return 0;
 498 }
 499
 500 static int disable_camera(struct bm2835_mmal_dev *dev)
 501 {
 502         int ret;
 503
 504         if (!dev->camera_use_count) {
 505                 v4l2_err(&dev->v4l2_dev,
 506                          "Disabled the camera when already disabled\n");
 507                 return -EINVAL;
 508         }
 509         dev->camera_use_count--;
 510         if (!dev->camera_use_count) {
 511                 unsigned int i = 0xFFFFFFFF;
 512
 513                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 514                          "Disabling camera\n");
 515                 ret =
 516                     vchiq_mmal_component_disable(
 517                                 dev->instance,
 518                                 dev->component[COMP_CAMERA]);
 519                 if (ret < 0) {
 520                         v4l2_err(&dev->v4l2_dev,
 521                                  "Failed disabling camera, ret %d\n", ret);
 522                         return -EINVAL;
 523                 }
 524                 vchiq_mmal_port_parameter_set(
 525                         dev->instance,
 526                         &dev->component[COMP_CAMERA]->control,
 527                         MMAL_PARAMETER_CAMERA_NUM, &i,
 528                         sizeof(i));
 529         }
 530         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 531                  "Camera refcount now %d\n", dev->camera_use_count);
 532         return 0;
 533 }
 534
 535 static void buffer_queue(struct vb2_buffer *vb)
 536 {
 537         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 538         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 539         struct vb2_mmal_buffer *buf =
 540                                 container_of(vb2, struct vb2_mmal_buffer, vb);
 541         int ret;
 542
 543         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 544                  "%s: dev:%p buf:%p, idx %u\n",
 545                  __func__, dev, buf, vb2->vb2_buf.index);
 546
 547         ret = vchiq_mmal_submit_buffer(dev->instance, dev->capture.port,
 548                                        &buf->mmal);
 549         if (ret < 0)
 550                 v4l2_err(&dev->v4l2_dev, "%s: error submitting buffer\n",
 551                          __func__);
 552 }
 553
 554 static int start_streaming(struct vb2_queue *vq, unsigned int count)
 555 {
 556         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
 557         int ret;
 558         u32 parameter_size;
 559
 560         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
 561                  __func__, dev);
 562
 563         /* ensure a format has actually been set */
 564         if (!dev->capture.port)
 565                 return -EINVAL;
 566
 567         if (enable_camera(dev) < 0) {
 568                 v4l2_err(&dev->v4l2_dev, "Failed to enable camera\n");
 569                 return -EINVAL;
 570         }
 571
 572         /*init_completion(&dev->capture.frame_cmplt); */
 573
 574         /* enable frame capture */
 575         dev->capture.frame_count = 1;
 576
 577         /* reset sequence number */
 578         dev->capture.sequence = 0;
 579
 580         /* if the preview is not already running, wait for a few frames for AGC
 581          * to settle down.
 582          */
 583         if (!dev->component[COMP_PREVIEW]->enabled)
 584                 msleep(300);
 585
 586         /* enable the connection from camera to encoder (if applicable) */
 587         if (dev->capture.camera_port != dev->capture.port &&
 588             dev->capture.camera_port) {
 589                 ret = vchiq_mmal_port_enable(dev->instance,
 590                                              dev->capture.camera_port, NULL);
 591                 if (ret) {
 592                         v4l2_err(&dev->v4l2_dev,
 593                                  "Failed to enable encode tunnel - error %d\n",
 594                                  ret);
 595                         return -1;
 596                 }
 597         }
 598
 599         /* Get VC timestamp at this point in time */
 600         parameter_size = sizeof(dev->capture.vc_start_timestamp);
 601         if (vchiq_mmal_port_parameter_get(dev->instance,
 602                                           dev->capture.camera_port,
 603                                           MMAL_PARAMETER_SYSTEM_TIME,
 604                                           &dev->capture.vc_start_timestamp,
 605                                           &parameter_size)) {
 606                 v4l2_err(&dev->v4l2_dev,
 607                          "Failed to get VC start time - update your VC f/w\n");
 608
 609                 /* Flag to indicate just to rely on kernel timestamps */
 610                 dev->capture.vc_start_timestamp = -1;
 611         } else {
 612                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 613                          "Start time %lld size %d\n",
 614                          dev->capture.vc_start_timestamp, parameter_size);
 615         }
 616
 617         dev->capture.kernel_start_ts = ktime_get();
 618         dev->capture.last_timestamp = 0;
 619
 620         /* enable the camera port */
 621         dev->capture.port->cb_ctx = dev;
 622         ret =
 623             vchiq_mmal_port_enable(dev->instance, dev->capture.port, buffer_cb);
 624         if (ret) {
 625                 v4l2_err(&dev->v4l2_dev,
 626                          "Failed to enable capture port - error %d. Disabling camera port again\n",
 627                          ret);
 628
 629                 vchiq_mmal_port_disable(dev->instance,
 630                                         dev->capture.camera_port);
 631                 if (disable_camera(dev) < 0) {
 632                         v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
 633                         return -EINVAL;
 634                 }
 635                 return -1;
 636         }
 637
 638         /* capture the first frame */
 639         vchiq_mmal_port_parameter_set(dev->instance,
 640                                       dev->capture.camera_port,
 641                                       MMAL_PARAMETER_CAPTURE,
 642                                       &dev->capture.frame_count,
 643                                       sizeof(dev->capture.frame_count));
 644         return 0;
 645 }
 646
 647 /* abort streaming and wait for last buffer */
 648 static void stop_streaming(struct vb2_queue *vq)
 649 {
 650         int ret;
 651         unsigned long timeout;
 652         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
 653         struct vchiq_mmal_port *port = dev->capture.port;
 654
 655         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
 656                  __func__, dev);
 657
 658         init_completion(&dev->capture.frame_cmplt);
 659         dev->capture.frame_count = 0;
 660
 661         /* ensure a format has actually been set */
 662         if (!port) {
 663                 v4l2_err(&dev->v4l2_dev,
 664                          "no capture port - stream not started?\n");
 665                 return;
 666         }
 667
 668         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "stopping capturing\n");
 669
 670         /* stop capturing frames */
 671         vchiq_mmal_port_parameter_set(dev->instance,
 672                                       dev->capture.camera_port,
 673                                       MMAL_PARAMETER_CAPTURE,
 674                                       &dev->capture.frame_count,
 675                                       sizeof(dev->capture.frame_count));
 676
 677         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 678                  "disabling connection\n");
 679
 680         /* disable the connection from camera to encoder */
 681         ret = vchiq_mmal_port_disable(dev->instance, dev->capture.camera_port);
 682         if (!ret && dev->capture.camera_port != port) {
 683                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 684                          "disabling port\n");
 685                 ret = vchiq_mmal_port_disable(dev->instance, port);
 686         } else if (dev->capture.camera_port != port) {
 687                 v4l2_err(&dev->v4l2_dev, "port_disable failed, error %d\n",
 688                          ret);
 689         }
 690
 691         /* wait for all buffers to be returned */
 692         while (atomic_read(&port->buffers_with_vpu)) {
 693                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 694                          "%s: Waiting for buffers to be returned - %d outstanding\n",
 695                          __func__, atomic_read(&port->buffers_with_vpu));
 696                 timeout = wait_for_completion_timeout(&dev->capture.frame_cmplt,
 697                                                       HZ);
 698                 if (timeout == 0) {
 699                         v4l2_err(&dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
 700                                  __func__,
 701                                  atomic_read(&port->buffers_with_vpu));
 702                         break;
 703                 }
 704         }
 705
 706         if (disable_camera(dev) < 0)
 707                 v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
 708 }
 709
 710 static const struct vb2_ops bm2835_mmal_video_qops = {
 711         .queue_setup = queue_setup,
 712         .buf_init = buffer_init,
 713         .buf_prepare = buffer_prepare,
 714         .buf_cleanup = buffer_cleanup,
 715         .buf_queue = buffer_queue,
 716         .start_streaming = start_streaming,
 717         .stop_streaming = stop_streaming,
 718         .wait_prepare = vb2_ops_wait_prepare,
 719         .wait_finish = vb2_ops_wait_finish,
 720 };
 721
 722 /* ------------------------------------------------------------------
 723  *      IOCTL operations
 724  * ------------------------------------------------------------------
 725  */
 726
 727 static int set_overlay_params(struct bm2835_mmal_dev *dev,
 728                               struct vchiq_mmal_port *port)
 729 {
 730         struct mmal_parameter_displayregion prev_config = {
 731                 .set =  MMAL_DISPLAY_SET_LAYER |
 732                         MMAL_DISPLAY_SET_ALPHA |
 733                         MMAL_DISPLAY_SET_DEST_RECT |
 734                         MMAL_DISPLAY_SET_FULLSCREEN,
 735                 .layer = PREVIEW_LAYER,
 736                 .alpha = dev->overlay.global_alpha,
 737                 .fullscreen = 0,
 738                 .dest_rect = {
 739                         .x = dev->overlay.w.left,
 740                         .y = dev->overlay.w.top,
 741                         .width = dev->overlay.w.width,
 742                         .height = dev->overlay.w.height,
 743                 },
 744         };
 745         return vchiq_mmal_port_parameter_set(dev->instance, port,
 746                                              MMAL_PARAMETER_DISPLAYREGION,
 747                                              &prev_config, sizeof(prev_config));
 748 }
 749
 750 /* overlay ioctl */
 751 static int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv,
 752                                        struct v4l2_fmtdesc *f)
 753 {
 754         struct mmal_fmt *fmt;
 755
 756         if (f->index >= ARRAY_SIZE(formats))
 757                 return -EINVAL;
 758
 759         fmt = &formats[f->index];
 760
 761         strlcpy((char *)f->description, fmt->name, sizeof(f->description));
 762         f->pixelformat = fmt->fourcc;
 763         f->flags = fmt->flags;
 764
 765         return 0;
 766 }
 767
 768 static int vidioc_g_fmt_vid_overlay(struct file *file, void *priv,
 769                                     struct v4l2_format *f)
 770 {
 771         struct bm2835_mmal_dev *dev = video_drvdata(file);
 772
 773         f->fmt.win = dev->overlay;
 774
 775         return 0;
 776 }
 777
 778 static int vidioc_try_fmt_vid_overlay(struct file *file, void *priv,
 779                                       struct v4l2_format *f)
 780 {
 781         struct bm2835_mmal_dev *dev = video_drvdata(file);
 782
 783         f->fmt.win.field = V4L2_FIELD_NONE;
 784         f->fmt.win.chromakey = 0;
 785         f->fmt.win.clips = NULL;
 786         f->fmt.win.clipcount = 0;
 787         f->fmt.win.bitmap = NULL;
 788
 789         v4l_bound_align_image(&f->fmt.win.w.width, MIN_WIDTH, dev->max_width, 1,
 790                               &f->fmt.win.w.height, MIN_HEIGHT, dev->max_height,
 791                               1, 0);
 792         v4l_bound_align_image(&f->fmt.win.w.left, MIN_WIDTH, dev->max_width, 1,
 793                               &f->fmt.win.w.top, MIN_HEIGHT, dev->max_height,
 794                               1, 0);
 795
 796         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 797                  "Overlay: Now w/h %dx%d l/t %dx%d\n",
 798                 f->fmt.win.w.width, f->fmt.win.w.height,
 799                 f->fmt.win.w.left, f->fmt.win.w.top);
 800
 801         v4l2_dump_win_format(1,
 802                              bcm2835_v4l2_debug,
 803                              &dev->v4l2_dev,
 804                              &f->fmt.win,
 805                              __func__);
 806         return 0;
 807 }
 808
 809 static int vidioc_s_fmt_vid_overlay(struct file *file, void *priv,
 810                                     struct v4l2_format *f)
 811 {
 812         struct bm2835_mmal_dev *dev = video_drvdata(file);
 813
 814         vidioc_try_fmt_vid_overlay(file, priv, f);
 815
 816         dev->overlay = f->fmt.win;
 817         if (dev->component[COMP_PREVIEW]->enabled) {
 818                 set_overlay_params(dev,
 819                                    &dev->component[COMP_PREVIEW]->input[0]);
 820         }
 821
 822         return 0;
 823 }
 824
 825 static int vidioc_overlay(struct file *file, void *f, unsigned int on)
 826 {
 827         int ret;
 828         struct bm2835_mmal_dev *dev = video_drvdata(file);
 829         struct vchiq_mmal_port *src;
 830         struct vchiq_mmal_port *dst;
 831
 832         if ((on && dev->component[COMP_PREVIEW]->enabled) ||
 833             (!on && !dev->component[COMP_PREVIEW]->enabled))
 834                 return 0;       /* already in requested state */
 835
 836         src =
 837             &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
 838
 839         if (!on) {
 840                 /* disconnect preview ports and disable component */
 841                 ret = vchiq_mmal_port_disable(dev->instance, src);
 842                 if (!ret)
 843                         ret =
 844                             vchiq_mmal_port_connect_tunnel(dev->instance, src,
 845                                                            NULL);
 846                 if (ret >= 0)
 847                         ret = vchiq_mmal_component_disable(
 848                                         dev->instance,
 849                                         dev->component[COMP_PREVIEW]);
 850
 851                 disable_camera(dev);
 852                 return ret;
 853         }
 854
 855         /* set preview port format and connect it to output */
 856         dst = &dev->component[COMP_PREVIEW]->input[0];
 857
 858         ret = vchiq_mmal_port_set_format(dev->instance, src);
 859         if (ret < 0)
 860                 return ret;
 861
 862         ret = set_overlay_params(dev, dst);
 863         if (ret < 0)
 864                 return ret;
 865
 866         if (enable_camera(dev) < 0) {
 867                 ret = -EINVAL;
 868                 return ret;
 869         }
 870
 871         ret = vchiq_mmal_component_enable(
 872                         dev->instance,
 873                         dev->component[COMP_PREVIEW]);
 874         if (ret < 0)
 875                 return ret;
 876
 877         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "connecting %p to %p\n",
 878                  src, dst);
 879         ret = vchiq_mmal_port_connect_tunnel(dev->instance, src, dst);
 880         if (!ret)
 881                 ret = vchiq_mmal_port_enable(dev->instance, src, NULL);
 882
 883         return ret;
 884 }
 885
 886 static int vidioc_g_fbuf(struct file *file, void *fh,
 887                          struct v4l2_framebuffer *a)
 888 {
 889         /* The video overlay must stay within the framebuffer and can't be
 890          * positioned independently.
 891          */
 892         struct bm2835_mmal_dev *dev = video_drvdata(file);
 893         struct vchiq_mmal_port *preview_port =
 894                 &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
 895
 896         a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
 897                         V4L2_FBUF_CAP_GLOBAL_ALPHA;
 898         a->flags = V4L2_FBUF_FLAG_OVERLAY;
 899         a->fmt.width = preview_port->es.video.width;
 900         a->fmt.height = preview_port->es.video.height;
 901         a->fmt.pixelformat = V4L2_PIX_FMT_YUV420;
 902         a->fmt.bytesperline = preview_port->es.video.width;
 903         a->fmt.sizeimage = (preview_port->es.video.width *
 904                                preview_port->es.video.height * 3) >> 1;
 905         a->fmt.colorspace = V4L2_COLORSPACE_SMPTE170M;
 906
 907         return 0;
 908 }
 909
 910 /* input ioctls */
 911 static int vidioc_enum_input(struct file *file, void *priv,
 912                              struct v4l2_input *inp)
 913 {
 914         /* only a single camera input */
 915         if (inp->index != 0)
 916                 return -EINVAL;
 917
 918         inp->type = V4L2_INPUT_TYPE_CAMERA;
 919         sprintf((char *)inp->name, "Camera %u", inp->index);
 920         return 0;
 921 }
 922
 923 static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 924 {
 925         *i = 0;
 926         return 0;
 927 }
 928
 929 static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 930 {
 931         if (i != 0)
 932                 return -EINVAL;
 933
 934         return 0;
 935 }
 936
 937 /* capture ioctls */
 938 static int vidioc_querycap(struct file *file, void *priv,
 939                            struct v4l2_capability *cap)
 940 {
 941         struct bm2835_mmal_dev *dev = video_drvdata(file);
 942         u32 major;
 943         u32 minor;
 944
 945         vchiq_mmal_version(dev->instance, &major, &minor);
 946
 947         strcpy((char *)cap->driver, "bm2835 mmal");
 948         snprintf((char *)cap->card, sizeof(cap->card), "mmal service %d.%d",
 949                  major, minor);
 950
 951         snprintf((char *)cap->bus_info, sizeof(cap->bus_info),
 952                  "platform:%s", dev->v4l2_dev.name);
 953         cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY |
 954             V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
 955         cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
 956
 957         return 0;
 958 }
 959
 960 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 961                                    struct v4l2_fmtdesc *f)
 962 {
 963         struct mmal_fmt *fmt;
 964
 965         if (f->index >= ARRAY_SIZE(formats))
 966                 return -EINVAL;
 967
 968         fmt = &formats[f->index];
 969
 970         strlcpy((char *)f->description, fmt->name, sizeof(f->description));
 971         f->pixelformat = fmt->fourcc;
 972         f->flags = fmt->flags;
 973
 974         return 0;
 975 }
 976
 977 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 978                                 struct v4l2_format *f)
 979 {
 980         struct bm2835_mmal_dev *dev = video_drvdata(file);
 981
 982         f->fmt.pix.width = dev->capture.width;
 983         f->fmt.pix.height = dev->capture.height;
 984         f->fmt.pix.field = V4L2_FIELD_NONE;
 985         f->fmt.pix.pixelformat = dev->capture.fmt->fourcc;
 986         f->fmt.pix.bytesperline = dev->capture.stride;
 987         f->fmt.pix.sizeimage = dev->capture.buffersize;
 988
 989         if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_RGB24)
 990                 f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
 991         else if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_JPEG)
 992                 f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
 993         else
 994                 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 995         f->fmt.pix.priv = 0;
 996
 997         v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
 998                              __func__);
 999         return 0;
1000 }
1001
1002 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
1003                                   struct v4l2_format *f)
1004 {
1005         struct bm2835_mmal_dev *dev = video_drvdata(file);
1006         struct mmal_fmt *mfmt;
1007
1008         mfmt = get_format(f);
1009         if (!mfmt) {
1010                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1011                          "Fourcc format (0x%08x) unknown.\n",
1012                          f->fmt.pix.pixelformat);
1013                 f->fmt.pix.pixelformat = formats[0].fourcc;
1014                 mfmt = get_format(f);
1015         }
1016
1017         f->fmt.pix.field = V4L2_FIELD_NONE;
1018
1019         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1020                  "Clipping/aligning %dx%d format %08X\n",
1021                  f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
1022
1023         v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, dev->max_width, 1,
1024                               &f->fmt.pix.height, MIN_HEIGHT, dev->max_height,
1025                               1, 0);
1026         f->fmt.pix.bytesperline = f->fmt.pix.width * mfmt->ybbp;
1027         if (!mfmt->remove_padding) {
1028                 if (mfmt->depth == 24) {
1029                         /*
1030                          * 24bpp is a pain as we can't use simple masking.
1031                          * Min stride is width aligned to 16, times 24bpp.
1032                          */
1033                         f->fmt.pix.bytesperline =
1034                                 ((f->fmt.pix.width + 15) & ~15) * 3;
1035                 } else {
1036                         /*
1037                          * GPU isn't removing padding, so stride is aligned to
1038                          * 32
1039                          */
1040                         int align_mask = ((32 * mfmt->depth) >> 3) - 1;
1041
1042                         f->fmt.pix.bytesperline =
1043                                 (f->fmt.pix.bytesperline + align_mask) &
1044                                                         ~align_mask;
1045                 }
1046                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1047                          "Not removing padding, so bytes/line = %d\n",
1048                          f->fmt.pix.bytesperline);
1049         }
1050
1051         /* Image buffer has to be padded to allow for alignment, even though
1052          * we sometimes then remove that padding before delivering the buffer.
1053          */
1054         f->fmt.pix.sizeimage = ((f->fmt.pix.height + 15) & ~15) *
1055                         (((f->fmt.pix.width + 31) & ~31) * mfmt->depth) >> 3;
1056
1057         if ((mfmt->flags & V4L2_FMT_FLAG_COMPRESSED) &&
1058             f->fmt.pix.sizeimage < MIN_BUFFER_SIZE)
1059                 f->fmt.pix.sizeimage = MIN_BUFFER_SIZE;
1060
1061         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24)
1062                 f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
1063         else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG)
1064                 f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
1065         else
1066                 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
1067         f->fmt.pix.priv = 0;
1068
1069         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1070                  "Now %dx%d format %08X\n",
1071                 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
1072
1073         v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
1074                              __func__);
1075         return 0;
1076 }
1077
1078 static int mmal_setup_components(struct bm2835_mmal_dev *dev,
1079                                  struct v4l2_format *f)
1080 {
1081         int ret;
1082         struct vchiq_mmal_port *port = NULL, *camera_port = NULL;
1083         struct vchiq_mmal_component *encode_component = NULL;
1084         struct mmal_fmt *mfmt = get_format(f);
1085         u32 remove_padding;
1086
1087         if (!mfmt)
1088                 return -EINVAL;
1089
1090         if (dev->capture.encode_component) {
1091                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1092                          "vid_cap - disconnect previous tunnel\n");
1093
1094                 /* Disconnect any previous connection */
1095                 vchiq_mmal_port_connect_tunnel(dev->instance,
1096                                                dev->capture.camera_port, NULL);
1097                 dev->capture.camera_port = NULL;
1098                 ret = vchiq_mmal_component_disable(dev->instance,
1099                                                    dev->capture.encode_component);
1100                 if (ret)
1101                         v4l2_err(&dev->v4l2_dev,
1102                                  "Failed to disable encode component %d\n",
1103                                  ret);
1104
1105                 dev->capture.encode_component = NULL;
1106         }
1107         /* format dependent port setup */
1108         switch (mfmt->mmal_component) {
1109         case COMP_CAMERA:
1110                 /* Make a further decision on port based on resolution */
1111                 if (f->fmt.pix.width <= max_video_width &&
1112                     f->fmt.pix.height <= max_video_height)
1113                         camera_port =
1114                             &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
1115                 else
1116                         camera_port =
1117                             &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
1118                 port = camera_port;
1119                 break;
1120         case COMP_IMAGE_ENCODE:
1121                 encode_component = dev->component[COMP_IMAGE_ENCODE];
1122                 port = &dev->component[COMP_IMAGE_ENCODE]->output[0];
1123                 camera_port =
1124                     &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
1125                 break;
1126         case COMP_VIDEO_ENCODE:
1127                 encode_component = dev->component[COMP_VIDEO_ENCODE];
1128                 port = &dev->component[COMP_VIDEO_ENCODE]->output[0];
1129                 camera_port =
1130                     &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
1131                 break;
1132         default:
1133                 break;
1134         }
1135
1136         if (!port)
1137                 return -EINVAL;
1138
1139         if (encode_component)
1140                 camera_port->format.encoding = MMAL_ENCODING_OPAQUE;
1141         else
1142                 camera_port->format.encoding = mfmt->mmal;
1143
1144         if (dev->rgb_bgr_swapped) {
1145                 if (camera_port->format.encoding == MMAL_ENCODING_RGB24)
1146                         camera_port->format.encoding = MMAL_ENCODING_BGR24;
1147                 else if (camera_port->format.encoding == MMAL_ENCODING_BGR24)
1148                         camera_port->format.encoding = MMAL_ENCODING_RGB24;
1149         }
1150
1151         remove_padding = mfmt->remove_padding;
1152         vchiq_mmal_port_parameter_set(dev->instance,
1153                                       camera_port,
1154                                       MMAL_PARAMETER_NO_IMAGE_PADDING,
1155                                       &remove_padding, sizeof(remove_padding));
1156
1157         camera_port->format.encoding_variant = 0;
1158         camera_port->es.video.width = f->fmt.pix.width;
1159         camera_port->es.video.height = f->fmt.pix.height;
1160         camera_port->es.video.crop.x = 0;
1161         camera_port->es.video.crop.y = 0;
1162         camera_port->es.video.crop.width = f->fmt.pix.width;
1163         camera_port->es.video.crop.height = f->fmt.pix.height;
1164         camera_port->es.video.frame_rate.num = 0;
1165         camera_port->es.video.frame_rate.den = 1;
1166         camera_port->es.video.color_space = MMAL_COLOR_SPACE_JPEG_JFIF;
1167
1168         ret = vchiq_mmal_port_set_format(dev->instance, camera_port);
1169
1170         if (!ret &&
1171             camera_port ==
1172             &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]) {
1173                 bool overlay_enabled =
1174                     !!dev->component[COMP_PREVIEW]->enabled;
1175                 struct vchiq_mmal_port *preview_port =
1176                     &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
1177                 /* Preview and encode ports need to match on resolution */
1178                 if (overlay_enabled) {
1179                         /* Need to disable the overlay before we can update
1180                          * the resolution
1181                          */
1182                         ret =
1183                             vchiq_mmal_port_disable(dev->instance,
1184                                                     preview_port);
1185                         if (!ret)
1186                                 ret =
1187                                     vchiq_mmal_port_connect_tunnel(
1188                                                 dev->instance,
1189                                                 preview_port,
1190                                                 NULL);
1191                 }
1192                 preview_port->es.video.width = f->fmt.pix.width;
1193                 preview_port->es.video.height = f->fmt.pix.height;
1194                 preview_port->es.video.crop.x = 0;
1195                 preview_port->es.video.crop.y = 0;
1196                 preview_port->es.video.crop.width = f->fmt.pix.width;
1197                 preview_port->es.video.crop.height = f->fmt.pix.height;
1198                 preview_port->es.video.frame_rate.num =
1199                                           dev->capture.timeperframe.denominator;
1200                 preview_port->es.video.frame_rate.den =
1201                                           dev->capture.timeperframe.numerator;
1202                 ret = vchiq_mmal_port_set_format(dev->instance, preview_port);
1203                 if (overlay_enabled) {
1204                         ret = vchiq_mmal_port_connect_tunnel(
1205                                 dev->instance,
1206                                 preview_port,
1207                                 &dev->component[COMP_PREVIEW]->input[0]);
1208                         if (!ret)
1209                                 ret = vchiq_mmal_port_enable(dev->instance,
1210                                                              preview_port,
1211                                                              NULL);
1212                 }
1213         }
1214
1215         if (ret) {
1216                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1217                          "%s failed to set format %dx%d %08X\n", __func__,
1218                          f->fmt.pix.width, f->fmt.pix.height,
1219                          f->fmt.pix.pixelformat);
1220                 /* ensure capture is not going to be tried */
1221                 dev->capture.port = NULL;
1222         } else {
1223                 if (encode_component) {
1224                         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1225                                  "vid_cap - set up encode comp\n");
1226
1227                         /* configure buffering */
1228                         camera_port->current_buffer.size =
1229                             camera_port->recommended_buffer.size;
1230                         camera_port->current_buffer.num =
1231                             camera_port->recommended_buffer.num;
1232
1233                         ret =
1234                             vchiq_mmal_port_connect_tunnel(
1235                                         dev->instance,
1236                                         camera_port,
1237                                         &encode_component->input[0]);
1238                         if (ret) {
1239                                 v4l2_dbg(1, bcm2835_v4l2_debug,
1240                                          &dev->v4l2_dev,
1241                                          "%s failed to create connection\n",
1242                                          __func__);
1243                                 /* ensure capture is not going to be tried */
1244                                 dev->capture.port = NULL;
1245                         } else {
1246                                 port->es.video.width = f->fmt.pix.width;
1247                                 port->es.video.height = f->fmt.pix.height;
1248                                 port->es.video.crop.x = 0;
1249                                 port->es.video.crop.y = 0;
1250                                 port->es.video.crop.width = f->fmt.pix.width;
1251                                 port->es.video.crop.height = f->fmt.pix.height;
1252                                 port->es.video.frame_rate.num =
1253                                           dev->capture.timeperframe.denominator;
1254                                 port->es.video.frame_rate.den =
1255                                           dev->capture.timeperframe.numerator;
1256
1257                                 port->format.encoding = mfmt->mmal;
1258                                 port->format.encoding_variant = 0;
1259                                 /* Set any encoding specific parameters */
1260                                 switch (mfmt->mmal_component) {
1261                                 case COMP_VIDEO_ENCODE:
1262                                         port->format.bitrate =
1263                                             dev->capture.encode_bitrate;
1264                                         break;
1265                                 case COMP_IMAGE_ENCODE:
1266                                         /* Could set EXIF parameters here */
1267                                         break;
1268                                 default:
1269                                         break;
1270                                 }
1271                                 ret = vchiq_mmal_port_set_format(dev->instance,
1272                                                                  port);
1273                                 if (ret)
1274                                         v4l2_dbg(1, bcm2835_v4l2_debug,
1275                                                  &dev->v4l2_dev,
1276                                                  "%s failed to set format %dx%d fmt %08X\n",
1277                                                  __func__,
1278                                                  f->fmt.pix.width,
1279                                                  f->fmt.pix.height,
1280                                                  f->fmt.pix.pixelformat
1281                                                  );
1282                         }
1283
1284                         if (!ret) {
1285                                 ret = vchiq_mmal_component_enable(
1286                                                 dev->instance,
1287                                                 encode_component);
1288                                 if (ret) {
1289                                         v4l2_dbg(1, bcm2835_v4l2_debug,
1290                                                  &dev->v4l2_dev,
1291                                                  "%s Failed to enable encode components\n",
1292                                                  __func__);
1293                                 }
1294                         }
1295                         if (!ret) {
1296                                 /* configure buffering */
1297                                 port->current_buffer.num = 1;
1298                                 port->current_buffer.size =
1299                                     f->fmt.pix.sizeimage;
1300                                 if (port->format.encoding ==
1301                                     MMAL_ENCODING_JPEG) {
1302                                         v4l2_dbg(1, bcm2835_v4l2_debug,
1303                                                  &dev->v4l2_dev,
1304                                                  "JPG - buf size now %d was %d\n",
1305                                                  f->fmt.pix.sizeimage,
1306                                                  port->current_buffer.size);
1307                                         port->current_buffer.size =
1308                                             (f->fmt.pix.sizeimage <
1309                                              (100 << 10)) ?
1310                                             (100 << 10) : f->fmt.pix.sizeimage;
1311                                 }
1312                                 v4l2_dbg(1, bcm2835_v4l2_debug,
1313                                          &dev->v4l2_dev,
1314                                          "vid_cap - cur_buf.size set to %d\n",
1315                                          f->fmt.pix.sizeimage);
1316                                 port->current_buffer.alignment = 0;
1317                         }
1318                 } else {
1319                         /* configure buffering */
1320                         camera_port->current_buffer.num = 1;
1321                         camera_port->current_buffer.size = f->fmt.pix.sizeimage;
1322                         camera_port->current_buffer.alignment = 0;
1323                 }
1324
1325                 if (!ret) {
1326                         dev->capture.fmt = mfmt;
1327                         dev->capture.stride = f->fmt.pix.bytesperline;
1328                         dev->capture.width = camera_port->es.video.crop.width;
1329                         dev->capture.height = camera_port->es.video.crop.height;
1330                         dev->capture.buffersize = port->current_buffer.size;
1331
1332                         /* select port for capture */
1333                         dev->capture.port = port;
1334                         dev->capture.camera_port = camera_port;
1335                         dev->capture.encode_component = encode_component;
1336                         v4l2_dbg(1, bcm2835_v4l2_debug,
1337                                  &dev->v4l2_dev,
1338                                 "Set dev->capture.fmt %08X, %dx%d, stride %d, size %d",
1339                                 port->format.encoding,
1340                                 dev->capture.width, dev->capture.height,
1341                                 dev->capture.stride, dev->capture.buffersize);
1342                 }
1343         }
1344
1345         /* todo: Need to convert the vchiq/mmal error into a v4l2 error. */
1346         return ret;
1347 }
1348
1349 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
1350                                 struct v4l2_format *f)
1351 {
1352         int ret;
1353         struct bm2835_mmal_dev *dev = video_drvdata(file);
1354         struct mmal_fmt *mfmt;
1355
1356         /* try the format to set valid parameters */
1357         ret = vidioc_try_fmt_vid_cap(file, priv, f);
1358         if (ret) {
1359                 v4l2_err(&dev->v4l2_dev,
1360                          "vid_cap - vidioc_try_fmt_vid_cap failed\n");
1361                 return ret;
1362         }
1363
1364         /* if a capture is running refuse to set format */
1365         if (vb2_is_busy(&dev->capture.vb_vidq)) {
1366                 v4l2_info(&dev->v4l2_dev, "%s device busy\n", __func__);
1367                 return -EBUSY;
1368         }
1369
1370         /* If the format is unsupported v4l2 says we should switch to
1371          * a supported one and not return an error.
1372          */
1373         mfmt = get_format(f);
1374         if (!mfmt) {
1375                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1376                          "Fourcc format (0x%08x) unknown.\n",
1377                          f->fmt.pix.pixelformat);
1378                 f->fmt.pix.pixelformat = formats[0].fourcc;
1379                 mfmt = get_format(f);
1380         }
1381
1382         ret = mmal_setup_components(dev, f);
1383         if (ret != 0) {
1384                 v4l2_err(&dev->v4l2_dev,
1385                          "%s: failed to setup mmal components: %d\n",
1386                          __func__, ret);
1387                 ret = -EINVAL;
1388         }
1389
1390         return ret;
1391 }
1392
1393 static int vidioc_enum_framesizes(struct file *file, void *fh,
1394                                   struct v4l2_frmsizeenum *fsize)
1395 {
1396         struct bm2835_mmal_dev *dev = video_drvdata(file);
1397         static const struct v4l2_frmsize_stepwise sizes = {
1398                 MIN_WIDTH, 0, 2,
1399                 MIN_HEIGHT, 0, 2
1400         };
1401         int i;
1402
1403         if (fsize->index)
1404                 return -EINVAL;
1405         for (i = 0; i < ARRAY_SIZE(formats); i++)
1406                 if (formats[i].fourcc == fsize->pixel_format)
1407                         break;
1408         if (i == ARRAY_SIZE(formats))
1409                 return -EINVAL;
1410         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
1411         fsize->stepwise = sizes;
1412         fsize->stepwise.max_width = dev->max_width;
1413         fsize->stepwise.max_height = dev->max_height;
1414         return 0;
1415 }
1416
1417 /* timeperframe is arbitrary and continuous */
1418 static int vidioc_enum_frameintervals(struct file *file, void *priv,
1419                                       struct v4l2_frmivalenum *fival)
1420 {
1421         struct bm2835_mmal_dev *dev = video_drvdata(file);
1422         int i;
1423
1424         if (fival->index)
1425                 return -EINVAL;
1426
1427         for (i = 0; i < ARRAY_SIZE(formats); i++)
1428                 if (formats[i].fourcc == fival->pixel_format)
1429                         break;
1430         if (i == ARRAY_SIZE(formats))
1431                 return -EINVAL;
1432
1433         /* regarding width & height - we support any within range */
1434         if (fival->width < MIN_WIDTH || fival->width > dev->max_width ||
1435             fival->height < MIN_HEIGHT || fival->height > dev->max_height)
1436                 return -EINVAL;
1437
1438         fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
1439
1440         /* fill in stepwise (step=1.0 is required by V4L2 spec) */
1441         fival->stepwise.min  = tpf_min;
1442         fival->stepwise.max  = tpf_max;
1443         fival->stepwise.step = (struct v4l2_fract) {1, 1};
1444
1445         return 0;
1446 }
1447
1448 static int vidioc_g_parm(struct file *file, void *priv,
1449                          struct v4l2_streamparm *parm)
1450 {
1451         struct bm2835_mmal_dev *dev = video_drvdata(file);
1452
1453         if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1454                 return -EINVAL;
1455
1456         parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
1457         parm->parm.capture.timeperframe = dev->capture.timeperframe;
1458         parm->parm.capture.readbuffers  = 1;
1459         return 0;
1460 }
1461
1462 #define FRACT_CMP(a, OP, b)     \
1463         ((u64)(a).numerator * (b).denominator  OP  \
1464          (u64)(b).numerator * (a).denominator)
1465
1466 static int vidioc_s_parm(struct file *file, void *priv,
1467                          struct v4l2_streamparm *parm)
1468 {
1469         struct bm2835_mmal_dev *dev = video_drvdata(file);
1470         struct v4l2_fract tpf;
1471
1472         if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1473                 return -EINVAL;
1474
1475         tpf = parm->parm.capture.timeperframe;
1476
1477         /* tpf: {*, 0} resets timing; clip to [min, max]*/
1478         tpf = tpf.denominator ? tpf : tpf_default;
1479         tpf = FRACT_CMP(tpf, <, tpf_min) ? tpf_min : tpf;
1480         tpf = FRACT_CMP(tpf, >, tpf_max) ? tpf_max : tpf;
1481
1482         dev->capture.timeperframe = tpf;
1483         parm->parm.capture.timeperframe = tpf;
1484         parm->parm.capture.readbuffers  = 1;
1485         parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
1486
1487         set_framerate_params(dev);
1488
1489         return 0;
1490 }
1491
1492 static const struct v4l2_ioctl_ops camera0_ioctl_ops = {
1493         /* overlay */
1494         .vidioc_enum_fmt_vid_overlay = vidioc_enum_fmt_vid_overlay,
1495         .vidioc_g_fmt_vid_overlay = vidioc_g_fmt_vid_overlay,
1496         .vidioc_try_fmt_vid_overlay = vidioc_try_fmt_vid_overlay,
1497         .vidioc_s_fmt_vid_overlay = vidioc_s_fmt_vid_overlay,
1498         .vidioc_overlay = vidioc_overlay,
1499         .vidioc_g_fbuf = vidioc_g_fbuf,
1500
1501         /* inputs */
1502         .vidioc_enum_input = vidioc_enum_input,
1503         .vidioc_g_input = vidioc_g_input,
1504         .vidioc_s_input = vidioc_s_input,
1505
1506         /* capture */
1507         .vidioc_querycap = vidioc_querycap,
1508         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
1509         .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
1510         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
1511         .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
1512
1513         /* buffer management */
1514         .vidioc_reqbufs = vb2_ioctl_reqbufs,
1515         .vidioc_create_bufs = vb2_ioctl_create_bufs,
1516         .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1517         .vidioc_querybuf = vb2_ioctl_querybuf,
1518         .vidioc_qbuf = vb2_ioctl_qbuf,
1519         .vidioc_dqbuf = vb2_ioctl_dqbuf,
1520         .vidioc_enum_framesizes = vidioc_enum_framesizes,
1521         .vidioc_enum_frameintervals = vidioc_enum_frameintervals,
1522         .vidioc_g_parm        = vidioc_g_parm,
1523         .vidioc_s_parm        = vidioc_s_parm,
1524         .vidioc_streamon = vb2_ioctl_streamon,
1525         .vidioc_streamoff = vb2_ioctl_streamoff,
1526
1527         .vidioc_log_status = v4l2_ctrl_log_status,
1528         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1529         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1530 };
1531
1532 /* ------------------------------------------------------------------
1533  *      Driver init/finalise
1534  * ------------------------------------------------------------------
1535  */
1536
1537 static const struct v4l2_file_operations camera0_fops = {
1538         .owner = THIS_MODULE,
1539         .open = v4l2_fh_open,
1540         .release = vb2_fop_release,
1541         .read = vb2_fop_read,
1542         .poll = vb2_fop_poll,
1543         .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
1544         .mmap = vb2_fop_mmap,
1545 };
1546
1547 static const struct video_device vdev_template = {
1548         .name = "camera0",
1549         .fops = &camera0_fops,
1550         .ioctl_ops = &camera0_ioctl_ops,
1551         .release = video_device_release_empty,
1552 };
1553
1554 /* Returns the number of cameras, and also the max resolution supported
1555  * by those cameras.
1556  */
1557 static int get_num_cameras(struct vchiq_mmal_instance *instance,
1558                            unsigned int resolutions[][2], int num_resolutions)
1559 {
1560         int ret;
1561         struct vchiq_mmal_component  *cam_info_component;
1562         struct mmal_parameter_camera_info_t cam_info = {0};
1563         u32 param_size = sizeof(cam_info);
1564         int i;
1565
1566         /* create a camera_info component */
1567         ret = vchiq_mmal_component_init(instance, "camera_info",
1568                                         &cam_info_component);
1569         if (ret < 0)
1570                 /* Unusual failure - let's guess one camera. */
1571                 return 1;
1572
1573         if (vchiq_mmal_port_parameter_get(instance,
1574                                           &cam_info_component->control,
1575                                           MMAL_PARAMETER_CAMERA_INFO,
1576                                           &cam_info,
1577                                           &param_size)) {
1578                 pr_info("Failed to get camera info\n");
1579         }
1580         for (i = 0;
1581              i < min_t(unsigned int, cam_info.num_cameras, num_resolutions);
1582              i++) {
1583                 resolutions[i][0] = cam_info.cameras[i].max_width;
1584                 resolutions[i][1] = cam_info.cameras[i].max_height;
1585         }
1586
1587         vchiq_mmal_component_finalise(instance,
1588                                       cam_info_component);
1589
1590         return cam_info.num_cameras;
1591 }
1592
1593 static int set_camera_parameters(struct vchiq_mmal_instance *instance,
1594                                  struct vchiq_mmal_component *camera,
1595                                  struct bm2835_mmal_dev *dev)
1596 {
1597         int ret;
1598         struct mmal_parameter_camera_config cam_config = {
1599                 .max_stills_w = dev->max_width,
1600                 .max_stills_h = dev->max_height,
1601                 .stills_yuv422 = 1,
1602                 .one_shot_stills = 1,
1603                 .max_preview_video_w = (max_video_width > 1920) ?
1604                                                 max_video_width : 1920,
1605                 .max_preview_video_h = (max_video_height > 1088) ?
1606                                                 max_video_height : 1088,
1607                 .num_preview_video_frames = 3,
1608                 .stills_capture_circular_buffer_height = 0,
1609                 .fast_preview_resume = 0,
1610                 .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
1611         };
1612
1613         ret = vchiq_mmal_port_parameter_set(instance, &camera->control,
1614                                             MMAL_PARAMETER_CAMERA_CONFIG,
1615                                             &cam_config, sizeof(cam_config));
1616         return ret;
1617 }
1618
1619 #define MAX_SUPPORTED_ENCODINGS 20
1620
1621 /* MMAL instance and component init */
1622 static int mmal_init(struct bm2835_mmal_dev *dev)
1623 {
1624         int ret;
1625         struct mmal_es_format_local *format;
1626         u32 supported_encodings[MAX_SUPPORTED_ENCODINGS];
1627         u32 param_size;
1628         struct vchiq_mmal_component  *camera;
1629
1630         ret = vchiq_mmal_init(&dev->instance);
1631         if (ret < 0) {
1632                 v4l2_err(&dev->v4l2_dev, "%s: vchiq mmal init failed %d\n",
1633                          __func__, ret);
1634                 return ret;
1635         }
1636
1637         /* get the camera component ready */
1638         ret = vchiq_mmal_component_init(dev->instance, "ril.camera",
1639                                         &dev->component[COMP_CAMERA]);
1640         if (ret < 0)
1641                 goto unreg_mmal;
1642
1643         camera = dev->component[COMP_CAMERA];
1644         if (camera->outputs < CAM_PORT_COUNT) {
1645                 v4l2_err(&dev->v4l2_dev, "%s: too few camera outputs %d needed %d\n",
1646                          __func__, camera->outputs, CAM_PORT_COUNT);
1647                 ret = -EINVAL;
1648                 goto unreg_camera;
1649         }
1650
1651         ret = set_camera_parameters(dev->instance,
1652                                     camera,
1653                                     dev);
1654         if (ret < 0) {
1655                 v4l2_err(&dev->v4l2_dev, "%s: unable to set camera parameters: %d\n",
1656                          __func__, ret);
1657                 goto unreg_camera;
1658         }
1659
1660         /* There was an error in the firmware that meant the camera component
1661          * produced BGR instead of RGB.
1662          * This is now fixed, but in order to support the old firmwares, we
1663          * have to check.
1664          */
1665         dev->rgb_bgr_swapped = true;
1666         param_size = sizeof(supported_encodings);
1667         ret = vchiq_mmal_port_parameter_get(dev->instance,
1668                                             &camera->output[CAM_PORT_CAPTURE],
1669                                             MMAL_PARAMETER_SUPPORTED_ENCODINGS,
1670                                             &supported_encodings,
1671                                             &param_size);
1672         if (ret == 0) {
1673                 int i;
1674
1675                 for (i = 0; i < param_size / sizeof(u32); i++) {
1676                         if (supported_encodings[i] == MMAL_ENCODING_BGR24) {
1677                                 /* Found BGR24 first - old firmware. */
1678                                 break;
1679                         }
1680                         if (supported_encodings[i] == MMAL_ENCODING_RGB24) {
1681                                 /* Found RGB24 first
1682                                  * new firmware, so use RGB24.
1683                                  */
1684                                 dev->rgb_bgr_swapped = false;
1685                         break;
1686                         }
1687                 }
1688         }
1689         format = &camera->output[CAM_PORT_PREVIEW].format;
1690
1691         format->encoding = MMAL_ENCODING_OPAQUE;
1692         format->encoding_variant = MMAL_ENCODING_I420;
1693
1694         format->es->video.width = 1024;
1695         format->es->video.height = 768;
1696         format->es->video.crop.x = 0;
1697         format->es->video.crop.y = 0;
1698         format->es->video.crop.width = 1024;
1699         format->es->video.crop.height = 768;
1700         format->es->video.frame_rate.num = 0; /* Rely on fps_range */
1701         format->es->video.frame_rate.den = 1;
1702
1703         format = &camera->output[CAM_PORT_VIDEO].format;
1704
1705         format->encoding = MMAL_ENCODING_OPAQUE;
1706         format->encoding_variant = MMAL_ENCODING_I420;
1707
1708         format->es->video.width = 1024;
1709         format->es->video.height = 768;
1710         format->es->video.crop.x = 0;
1711         format->es->video.crop.y = 0;
1712         format->es->video.crop.width = 1024;
1713         format->es->video.crop.height = 768;
1714         format->es->video.frame_rate.num = 0; /* Rely on fps_range */
1715         format->es->video.frame_rate.den = 1;
1716
1717         format = &camera->output[CAM_PORT_CAPTURE].format;
1718
1719         format->encoding = MMAL_ENCODING_OPAQUE;
1720
1721         format->es->video.width = 2592;
1722         format->es->video.height = 1944;
1723         format->es->video.crop.x = 0;
1724         format->es->video.crop.y = 0;
1725         format->es->video.crop.width = 2592;
1726         format->es->video.crop.height = 1944;
1727         format->es->video.frame_rate.num = 0; /* Rely on fps_range */
1728         format->es->video.frame_rate.den = 1;
1729
1730         dev->capture.width = format->es->video.width;
1731         dev->capture.height = format->es->video.height;
1732         dev->capture.fmt = &formats[0];
1733         dev->capture.encode_component = NULL;
1734         dev->capture.timeperframe = tpf_default;
1735         dev->capture.enc_profile = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH;
1736         dev->capture.enc_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
1737
1738         /* get the preview component ready */
1739         ret = vchiq_mmal_component_init(
1740                         dev->instance, "ril.video_render",
1741                         &dev->component[COMP_PREVIEW]);
1742         if (ret < 0)
1743                 goto unreg_camera;
1744
1745         if (dev->component[COMP_PREVIEW]->inputs < 1) {
1746                 ret = -EINVAL;
1747                 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
1748                          __func__, dev->component[COMP_PREVIEW]->inputs, 1);
1749                 goto unreg_preview;
1750         }
1751
1752         /* get the image encoder component ready */
1753         ret = vchiq_mmal_component_init(
1754                 dev->instance, "ril.image_encode",
1755                 &dev->component[COMP_IMAGE_ENCODE]);
1756         if (ret < 0)
1757                 goto unreg_preview;
1758
1759         if (dev->component[COMP_IMAGE_ENCODE]->inputs < 1) {
1760                 ret = -EINVAL;
1761                 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
1762                          __func__, dev->component[COMP_IMAGE_ENCODE]->inputs,
1763                          1);
1764                 goto unreg_image_encoder;
1765         }
1766
1767         /* get the video encoder component ready */
1768         ret = vchiq_mmal_component_init(dev->instance, "ril.video_encode",
1769                                         &dev->component[COMP_VIDEO_ENCODE]);
1770         if (ret < 0)
1771                 goto unreg_image_encoder;
1772
1773         if (dev->component[COMP_VIDEO_ENCODE]->inputs < 1) {
1774                 ret = -EINVAL;
1775                 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
1776                          __func__, dev->component[COMP_VIDEO_ENCODE]->inputs,
1777                          1);
1778                 goto unreg_vid_encoder;
1779         }
1780
1781         {
1782                 struct vchiq_mmal_port *encoder_port =
1783                         &dev->component[COMP_VIDEO_ENCODE]->output[0];
1784                 encoder_port->format.encoding = MMAL_ENCODING_H264;
1785                 ret = vchiq_mmal_port_set_format(dev->instance,
1786                                                  encoder_port);
1787         }
1788
1789         {
1790                 unsigned int enable = 1;
1791
1792                 vchiq_mmal_port_parameter_set(
1793                         dev->instance,
1794                         &dev->component[COMP_VIDEO_ENCODE]->control,
1795                         MMAL_PARAMETER_VIDEO_IMMUTABLE_INPUT,
1796                         &enable, sizeof(enable));
1797
1798                 vchiq_mmal_port_parameter_set(dev->instance,
1799                                               &dev->component[COMP_VIDEO_ENCODE]->control,
1800                                               MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
1801                                               &enable,
1802                                               sizeof(enable));
1803         }
1804         ret = bm2835_mmal_set_all_camera_controls(dev);
1805         if (ret < 0) {
1806                 v4l2_err(&dev->v4l2_dev, "%s: failed to set all camera controls: %d\n",
1807                          __func__, ret);
1808                 goto unreg_vid_encoder;
1809         }
1810
1811         return 0;
1812
1813 unreg_vid_encoder:
1814         pr_err("Cleanup: Destroy video encoder\n");
1815         vchiq_mmal_component_finalise(
1816                 dev->instance,
1817                 dev->component[COMP_VIDEO_ENCODE]);
1818
1819 unreg_image_encoder:
1820         pr_err("Cleanup: Destroy image encoder\n");
1821         vchiq_mmal_component_finalise(
1822                 dev->instance,
1823                 dev->component[COMP_IMAGE_ENCODE]);
1824
1825 unreg_preview:
1826         pr_err("Cleanup: Destroy video render\n");
1827         vchiq_mmal_component_finalise(dev->instance,
1828                                       dev->component[COMP_PREVIEW]);
1829
1830 unreg_camera:
1831         pr_err("Cleanup: Destroy camera\n");
1832         vchiq_mmal_component_finalise(dev->instance,
1833                                       dev->component[COMP_CAMERA]);
1834
1835 unreg_mmal:
1836         vchiq_mmal_finalise(dev->instance);
1837         return ret;
1838 }
1839
1840 static int bm2835_mmal_init_device(struct bm2835_mmal_dev *dev,
1841                                    struct video_device *vfd)
1842 {
1843         int ret;
1844
1845         *vfd = vdev_template;
1846
1847         vfd->v4l2_dev = &dev->v4l2_dev;
1848
1849         vfd->lock = &dev->mutex;
1850
1851         vfd->queue = &dev->capture.vb_vidq;
1852
1853         /* video device needs to be able to access instance data */
1854         video_set_drvdata(vfd, dev);
1855
1856         ret = video_register_device(vfd,
1857                                     VFL_TYPE_GRABBER,
1858                                     video_nr[dev->camera_num]);
1859         if (ret < 0)
1860                 return ret;
1861
1862         v4l2_info(vfd->v4l2_dev,
1863                   "V4L2 device registered as %s - stills mode > %dx%d\n",
1864                   video_device_node_name(vfd),
1865                   max_video_width, max_video_height);
1866
1867         return 0;
1868 }
1869
1870 static void bcm2835_cleanup_instance(struct bm2835_mmal_dev *dev)
1871 {
1872         if (!dev)
1873                 return;
1874
1875         v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
1876                   video_device_node_name(&dev->vdev));
1877
1878         video_unregister_device(&dev->vdev);
1879
1880         if (dev->capture.encode_component) {
1881                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
1882                          "mmal_exit - disconnect tunnel\n");
1883                 vchiq_mmal_port_connect_tunnel(dev->instance,
1884                                                dev->capture.camera_port, NULL);
1885                 vchiq_mmal_component_disable(dev->instance,
1886                                              dev->capture.encode_component);
1887         }
1888         vchiq_mmal_component_disable(dev->instance,
1889                                      dev->component[COMP_CAMERA]);
1890
1891         vchiq_mmal_component_finalise(dev->instance,
1892                                       dev->component[COMP_VIDEO_ENCODE]);
1893
1894         vchiq_mmal_component_finalise(dev->instance,
1895                                       dev->component[COMP_IMAGE_ENCODE]);
1896
1897         vchiq_mmal_component_finalise(dev->instance,
1898                                       dev->component[COMP_PREVIEW]);
1899
1900         vchiq_mmal_component_finalise(dev->instance,
1901                                       dev->component[COMP_CAMERA]);
1902
1903         v4l2_ctrl_handler_free(&dev->ctrl_handler);
1904
1905         v4l2_device_unregister(&dev->v4l2_dev);
1906
1907         kfree(dev);
1908 }
1909
1910 static struct v4l2_format default_v4l2_format = {
1911         .fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG,
1912         .fmt.pix.width = 1024,
1913         .fmt.pix.bytesperline = 0,
1914         .fmt.pix.height = 768,
1915         .fmt.pix.sizeimage = 1024 * 768,
1916 };
1917
1918 static int bcm2835_mmal_probe(struct platform_device *pdev)
1919 {
1920         int ret;
1921         struct bm2835_mmal_dev *dev;
1922         struct vb2_queue *q;
1923         int camera;
1924         unsigned int num_cameras;
1925         struct vchiq_mmal_instance *instance;
1926         unsigned int resolutions[MAX_BCM2835_CAMERAS][2];
1927         int i;
1928
1929         ret = vchiq_mmal_init(&instance);
1930         if (ret < 0)
1931                 return ret;
1932
1933         num_cameras = get_num_cameras(instance,
1934                                       resolutions,
1935                                       MAX_BCM2835_CAMERAS);
1936
1937         if (num_cameras < 1) {
1938                 ret = -ENODEV;
1939                 goto cleanup_mmal;
1940         }
1941
1942         if (num_cameras > MAX_BCM2835_CAMERAS)
1943                 num_cameras = MAX_BCM2835_CAMERAS;
1944
1945         for (camera = 0; camera < num_cameras; camera++) {
1946                 dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1947                 if (!dev) {
1948                         ret = -ENOMEM;
1949                         goto cleanup_gdev;
1950                 }
1951
1952                 /* v4l2 core mutex used to protect all fops and v4l2 ioctls. */
1953                 mutex_init(&dev->mutex);
1954                 dev->camera_num = camera;
1955                 dev->max_width = resolutions[camera][0];
1956                 dev->max_height = resolutions[camera][1];
1957
1958                 /* setup device defaults */
1959                 dev->overlay.w.left = 150;
1960                 dev->overlay.w.top = 50;
1961                 dev->overlay.w.width = 1024;
1962                 dev->overlay.w.height = 768;
1963                 dev->overlay.clipcount = 0;
1964                 dev->overlay.field = V4L2_FIELD_NONE;
1965                 dev->overlay.global_alpha = 255;
1966
1967                 dev->capture.fmt = &formats[3]; /* JPEG */
1968
1969                 /* v4l device registration */
1970                 snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name),
1971                          "%s", BM2835_MMAL_MODULE_NAME);
1972                 ret = v4l2_device_register(NULL, &dev->v4l2_dev);
1973                 if (ret) {
1974                         dev_err(&pdev->dev, "%s: could not register V4L2 device: %d\n",
1975                                 __func__, ret);
1976                         goto free_dev;
1977                 }
1978
1979                 /* setup v4l controls */
1980                 ret = bm2835_mmal_init_controls(dev, &dev->ctrl_handler);
1981                 if (ret < 0) {
1982                         v4l2_err(&dev->v4l2_dev, "%s: could not init controls: %d\n",
1983                                  __func__, ret);
1984                         goto unreg_dev;
1985                 }
1986                 dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler;
1987
1988                 /* mmal init */
1989                 dev->instance = instance;
1990                 ret = mmal_init(dev);
1991                 if (ret < 0) {
1992                         v4l2_err(&dev->v4l2_dev, "%s: mmal init failed: %d\n",
1993                                  __func__, ret);
1994                         goto unreg_dev;
1995                 }
1996                 /* initialize queue */
1997                 q = &dev->capture.vb_vidq;
1998                 memset(q, 0, sizeof(*q));
1999                 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
2000                 q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
2001                 q->drv_priv = dev;
2002                 q->buf_struct_size = sizeof(struct vb2_mmal_buffer);
2003                 q->ops = &bm2835_mmal_video_qops;
2004                 q->mem_ops = &vb2_vmalloc_memops;
2005                 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
2006                 q->lock = &dev->mutex;
2007                 ret = vb2_queue_init(q);
2008                 if (ret < 0)
2009                         goto unreg_dev;
2010
2011                 /* initialise video devices */
2012                 ret = bm2835_mmal_init_device(dev, &dev->vdev);
2013                 if (ret < 0) {
2014                         v4l2_err(&dev->v4l2_dev, "%s: could not init device: %d\n",
2015                                  __func__, ret);
2016                         goto unreg_dev;
2017                 }
2018
2019                 /* Really want to call vidioc_s_fmt_vid_cap with the default
2020                  * format, but currently the APIs don't join up.
2021                  */
2022                 ret = mmal_setup_components(dev, &default_v4l2_format);
2023                 if (ret < 0) {
2024                         v4l2_err(&dev->v4l2_dev, "%s: could not setup components: %d\n",
2025                                  __func__, ret);
2026                         goto unreg_dev;
2027                 }
2028
2029                 v4l2_info(&dev->v4l2_dev,
2030                           "Broadcom 2835 MMAL video capture ver %s loaded.\n",
2031                           BM2835_MMAL_VERSION);
2032
2033                 gdev[camera] = dev;
2034         }
2035         return 0;
2036
2037 unreg_dev:
2038         v4l2_ctrl_handler_free(&dev->ctrl_handler);
2039         v4l2_device_unregister(&dev->v4l2_dev);
2040
2041 free_dev:
2042         kfree(dev);
2043
2044 cleanup_gdev:
2045         for (i = 0; i < camera; i++) {
2046                 bcm2835_cleanup_instance(gdev[i]);
2047                 gdev[i] = NULL;
2048         }
2049
2050 cleanup_mmal:
2051         vchiq_mmal_finalise(instance);
2052
2053         return ret;
2054 }
2055
2056 static int bcm2835_mmal_remove(struct platform_device *pdev)
2057 {
2058         int camera;
2059         struct vchiq_mmal_instance *instance = gdev[0]->instance;
2060
2061         for (camera = 0; camera < MAX_BCM2835_CAMERAS; camera++) {
2062                 bcm2835_cleanup_instance(gdev[camera]);
2063                 gdev[camera] = NULL;
2064         }
2065         vchiq_mmal_finalise(instance);
2066
2067         return 0;
2068 }
2069
2070 static struct platform_driver bcm2835_camera_driver = {
2071         .probe          = bcm2835_mmal_probe,
2072         .remove         = bcm2835_mmal_remove,
2073         .driver         = {
2074                 .name   = "bcm2835-camera",
2075         },
2076 };
2077
2078 module_platform_driver(bcm2835_camera_driver)
2079
2080 MODULE_DESCRIPTION("Broadcom 2835 MMAL video capture");
2081 MODULE_AUTHOR("Vincent Sanders");
2082 MODULE_LICENSE("GPL");
2083 MODULE_VERSION(BM2835_MMAL_VERSION);
2084 MODULE_ALIAS("platform:bcm2835-camera");