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