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

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