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