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