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