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