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

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