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

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