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