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