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