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

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