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