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