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