linux/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
<<
>>
Prefs
   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
  46MODULE_DESCRIPTION("Broadcom 2835 MMAL video capture");
  47MODULE_AUTHOR("Vincent Sanders");
  48MODULE_LICENSE("GPL");
  49MODULE_VERSION(BM2835_MMAL_VERSION);
  50
  51int bcm2835_v4l2_debug;
  52module_param_named(debug, bcm2835_v4l2_debug, int, 0644);
  53MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2");
  54
  55#define UNSET (-1)
  56static int video_nr[] = {[0 ... (MAX_BCM2835_CAMERAS - 1)] = UNSET };
  57module_param_array(video_nr, int, NULL, 0644);
  58MODULE_PARM_DESC(video_nr, "videoX start numbers, -1 is autodetect");
  59
  60static int max_video_width = MAX_VIDEO_MODE_WIDTH;
  61static int max_video_height = MAX_VIDEO_MODE_HEIGHT;
  62module_param(max_video_width, int, 0644);
  63MODULE_PARM_DESC(max_video_width, "Threshold for video mode");
  64module_param(max_video_height, int, 0644);
  65MODULE_PARM_DESC(max_video_height, "Threshold for video mode");
  66
  67/* global device data array */
  68static 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 */
  74static 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 */
  80static 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
 210static 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
 229static 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
 270static 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
 284static 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
 306static 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
 317static 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
 324static 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 (buf) {
 346                        /* this should only ever happen if the port is
 347                         * disabled and there are buffers still queued
 348                         */
 349                        vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 350                        pr_debug("Empty buffer");
 351                } else if (dev->capture.frame_count) {
 352                        /* grab another frame */
 353                        if (is_capturing(dev)) {
 354                                pr_debug("Grab another frame");
 355                                vchiq_mmal_port_parameter_set(
 356                                        instance,
 357                                        dev->capture.camera_port,
 358                                        MMAL_PARAMETER_CAPTURE,
 359                                        &dev->capture.frame_count,
 360                                        sizeof(dev->capture.frame_count));
 361                        }
 362                } else {
 363                        /* signal frame completion */
 364                        complete(&dev->capture.frame_cmplt);
 365                }
 366        } else {
 367                if (dev->capture.frame_count) {
 368                        if (dev->capture.vc_start_timestamp != -1 &&
 369                            pts != 0) {
 370                                ktime_t timestamp;
 371                                s64 runtime_us = pts -
 372                                    dev->capture.vc_start_timestamp;
 373                                timestamp = ktime_add_us(dev->capture.kernel_start_ts,
 374                                                         runtime_us);
 375                                v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 376                                         "Convert start time %llu and %llu with offset %llu to %llu\n",
 377                                         ktime_to_ns(dev->capture.kernel_start_ts),
 378                                         dev->capture.vc_start_timestamp, pts,
 379                                         ktime_to_ns(timestamp));
 380                                buf->vb.vb2_buf.timestamp = ktime_to_ns(timestamp);
 381                        } else {
 382                                buf->vb.vb2_buf.timestamp = ktime_get_ns();
 383                        }
 384
 385                        vb2_set_plane_payload(&buf->vb.vb2_buf, 0, length);
 386                        vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 387
 388                        if (mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS &&
 389                            is_capturing(dev)) {
 390                                v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 391                                         "Grab another frame as buffer has EOS");
 392                                vchiq_mmal_port_parameter_set(
 393                                        instance,
 394                                        dev->capture.camera_port,
 395                                        MMAL_PARAMETER_CAPTURE,
 396                                        &dev->capture.frame_count,
 397                                        sizeof(dev->capture.frame_count));
 398                        }
 399                } else {
 400                        /* signal frame completion */
 401                        vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 402                        complete(&dev->capture.frame_cmplt);
 403                }
 404        }
 405}
 406
 407static int enable_camera(struct bm2835_mmal_dev *dev)
 408{
 409        int ret;
 410
 411        if (!dev->camera_use_count) {
 412                ret = vchiq_mmal_port_parameter_set(
 413                        dev->instance,
 414                        &dev->component[MMAL_COMPONENT_CAMERA]->control,
 415                        MMAL_PARAMETER_CAMERA_NUM, &dev->camera_num,
 416                        sizeof(dev->camera_num));
 417                if (ret < 0) {
 418                        v4l2_err(&dev->v4l2_dev,
 419                                 "Failed setting camera num, ret %d\n", ret);
 420                        return -EINVAL;
 421                }
 422
 423                ret = vchiq_mmal_component_enable(
 424                                dev->instance,
 425                                dev->component[MMAL_COMPONENT_CAMERA]);
 426                if (ret < 0) {
 427                        v4l2_err(&dev->v4l2_dev,
 428                                 "Failed enabling camera, ret %d\n", ret);
 429                        return -EINVAL;
 430                }
 431        }
 432        dev->camera_use_count++;
 433        v4l2_dbg(1, bcm2835_v4l2_debug,
 434                 &dev->v4l2_dev, "enabled camera (refcount %d)\n",
 435                        dev->camera_use_count);
 436        return 0;
 437}
 438
 439static int disable_camera(struct bm2835_mmal_dev *dev)
 440{
 441        int ret;
 442
 443        if (!dev->camera_use_count) {
 444                v4l2_err(&dev->v4l2_dev,
 445                         "Disabled the camera when already disabled\n");
 446                return -EINVAL;
 447        }
 448        dev->camera_use_count--;
 449        if (!dev->camera_use_count) {
 450                unsigned int i = 0xFFFFFFFF;
 451
 452                v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 453                         "Disabling camera\n");
 454                ret =
 455                    vchiq_mmal_component_disable(
 456                                dev->instance,
 457                                dev->component[MMAL_COMPONENT_CAMERA]);
 458                if (ret < 0) {
 459                        v4l2_err(&dev->v4l2_dev,
 460                                 "Failed disabling camera, ret %d\n", ret);
 461                        return -EINVAL;
 462                }
 463                vchiq_mmal_port_parameter_set(
 464                        dev->instance,
 465                        &dev->component[MMAL_COMPONENT_CAMERA]->control,
 466                        MMAL_PARAMETER_CAMERA_NUM, &i,
 467                        sizeof(i));
 468        }
 469        v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 470                 "Camera refcount now %d\n", dev->camera_use_count);
 471        return 0;
 472}
 473
 474static void buffer_queue(struct vb2_buffer *vb)
 475{
 476        struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
 477        struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 478        struct mmal_buffer *buf = container_of(vb2, struct mmal_buffer, vb);
 479        int ret;
 480
 481        v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 482                 "%s: dev:%p buf:%p, idx %u\n",
 483                 __func__, dev, buf, vb2->vb2_buf.index);
 484
 485        ret = vchiq_mmal_submit_buffer(dev->instance, dev->capture.port, buf);
 486        if (ret < 0)
 487                v4l2_err(&dev->v4l2_dev, "%s: error submitting buffer\n",
 488                         __func__);
 489}
 490
 491static int start_streaming(struct vb2_queue *vq, unsigned int count)
 492{
 493        struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
 494        int ret;
 495        u32 parameter_size;
 496
 497        v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
 498                 __func__, dev);
 499
 500        /* ensure a format has actually been set */
 501        if (!dev->capture.port)
 502                return -EINVAL;
 503
 504        if (enable_camera(dev) < 0) {
 505                v4l2_err(&dev->v4l2_dev, "Failed to enable camera\n");
 506                return -EINVAL;
 507        }
 508
 509        /*init_completion(&dev->capture.frame_cmplt); */
 510
 511        /* enable frame capture */
 512        dev->capture.frame_count = 1;
 513
 514        /* if the preview is not already running, wait for a few frames for AGC
 515         * to settle down.
 516         */
 517        if (!dev->component[MMAL_COMPONENT_PREVIEW]->enabled)
 518                msleep(300);
 519
 520        /* enable the connection from camera to encoder (if applicable) */
 521        if (dev->capture.camera_port != dev->capture.port
 522            && dev->capture.camera_port) {
 523                ret = vchiq_mmal_port_enable(dev->instance,
 524                                             dev->capture.camera_port, NULL);
 525                if (ret) {
 526                        v4l2_err(&dev->v4l2_dev,
 527                                 "Failed to enable encode tunnel - error %d\n",
 528                                 ret);
 529                        return -1;
 530                }
 531        }
 532
 533        /* Get VC timestamp at this point in time */
 534        parameter_size = sizeof(dev->capture.vc_start_timestamp);
 535        if (vchiq_mmal_port_parameter_get(dev->instance,
 536                                          dev->capture.camera_port,
 537                                          MMAL_PARAMETER_SYSTEM_TIME,
 538                                          &dev->capture.vc_start_timestamp,
 539                                          &parameter_size)) {
 540                v4l2_err(&dev->v4l2_dev,
 541                         "Failed to get VC start time - update your VC f/w\n");
 542
 543                /* Flag to indicate just to rely on kernel timestamps */
 544                dev->capture.vc_start_timestamp = -1;
 545        } else
 546                v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 547                         "Start time %lld size %d\n",
 548                         dev->capture.vc_start_timestamp, parameter_size);
 549
 550        dev->capture.kernel_start_ts = ktime_get();
 551
 552        /* enable the camera port */
 553        dev->capture.port->cb_ctx = dev;
 554        ret =
 555            vchiq_mmal_port_enable(dev->instance, dev->capture.port, buffer_cb);
 556        if (ret) {
 557                v4l2_err(&dev->v4l2_dev,
 558                        "Failed to enable capture port - error %d. Disabling camera port again\n",
 559                        ret);
 560
 561                vchiq_mmal_port_disable(dev->instance,
 562                                        dev->capture.camera_port);
 563                if (disable_camera(dev) < 0) {
 564                        v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
 565                        return -EINVAL;
 566                }
 567                return -1;
 568        }
 569
 570        /* capture the first frame */
 571        vchiq_mmal_port_parameter_set(dev->instance,
 572                                      dev->capture.camera_port,
 573                                      MMAL_PARAMETER_CAPTURE,
 574                                      &dev->capture.frame_count,
 575                                      sizeof(dev->capture.frame_count));
 576        return 0;
 577}
 578
 579/* abort streaming and wait for last buffer */
 580static void stop_streaming(struct vb2_queue *vq)
 581{
 582        int ret;
 583        struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
 584
 585        v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
 586                 __func__, dev);
 587
 588        init_completion(&dev->capture.frame_cmplt);
 589        dev->capture.frame_count = 0;
 590
 591        /* ensure a format has actually been set */
 592        if (!dev->capture.port) {
 593                v4l2_err(&dev->v4l2_dev,
 594                         "no capture port - stream not started?\n");
 595                return;
 596        }
 597
 598        v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "stopping capturing\n");
 599
 600        /* stop capturing frames */
 601        vchiq_mmal_port_parameter_set(dev->instance,
 602                                      dev->capture.camera_port,
 603                                      MMAL_PARAMETER_CAPTURE,
 604                                      &dev->capture.frame_count,
 605                                      sizeof(dev->capture.frame_count));
 606
 607        /* wait for last frame to complete */
 608        ret = wait_for_completion_timeout(&dev->capture.frame_cmplt, HZ);
 609        if (ret <= 0)
 610                v4l2_err(&dev->v4l2_dev,
 611                         "error %d waiting for frame completion\n", ret);
 612
 613        v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 614                 "disabling connection\n");
 615
 616        /* disable the connection from camera to encoder */
 617        ret = vchiq_mmal_port_disable(dev->instance, dev->capture.camera_port);
 618        if (!ret && dev->capture.camera_port != dev->capture.port) {
 619                v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 620                         "disabling port\n");
 621                ret = vchiq_mmal_port_disable(dev->instance, dev->capture.port);
 622        } else if (dev->capture.camera_port != dev->capture.port) {
 623                v4l2_err(&dev->v4l2_dev, "port_disable failed, error %d\n",
 624                         ret);
 625        }
 626
 627        if (disable_camera(dev) < 0)
 628                v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
 629}
 630
 631static void bm2835_mmal_lock(struct vb2_queue *vq)
 632{
 633        struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
 634
 635        mutex_lock(&dev->mutex);
 636}
 637
 638static void bm2835_mmal_unlock(struct vb2_queue *vq)
 639{
 640        struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
 641
 642        mutex_unlock(&dev->mutex);
 643}
 644
 645static 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 = bm2835_mmal_unlock,
 654        .wait_finish = bm2835_mmal_lock,
 655};
 656
 657/* ------------------------------------------------------------------
 658 *      IOCTL operations
 659 * ------------------------------------------------------------------
 660 */
 661
 662static 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 */
 686static 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
 703static 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
 713static 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
 744static 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
 760static 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);
 816error:
 817        return ret;
 818}
 819
 820static 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 */
 846static 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
 858static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 859{
 860        *i = 0;
 861        return 0;
 862}
 863
 864static 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 */
 873static 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
 895static 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
 912static 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
 937static 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
1000static 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
1278static 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
1322static 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 */
1347static 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
1377static 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
1395static 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
1421static 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
1466static 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
1476static 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 */
1486static 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
1522static 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 */
1551static 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
1732unreg_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
1738unreg_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
1744unreg_preview:
1745        pr_err("Cleanup: Destroy video render\n");
1746        vchiq_mmal_component_finalise(dev->instance,
1747                                      dev->component[MMAL_COMPONENT_PREVIEW]);
1748
1749unreg_camera:
1750        pr_err("Cleanup: Destroy camera\n");
1751        vchiq_mmal_component_finalise(dev->instance,
1752                                      dev->component[MMAL_COMPONENT_CAMERA]);
1753
1754unreg_mmal:
1755        vchiq_mmal_finalise(dev->instance);
1756        return ret;
1757}
1758
1759static 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
1789static 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
1831static 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
1839static 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        if (num_cameras > MAX_BCM2835_CAMERAS)
1858                num_cameras = MAX_BCM2835_CAMERAS;
1859
1860        for (camera = 0; camera < num_cameras; camera++) {
1861                dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1862                if (!dev) {
1863                        ret = -ENOMEM;
1864                        goto cleanup_gdev;
1865                }
1866
1867                dev->camera_num = camera;
1868                dev->max_width = resolutions[camera][0];
1869                dev->max_height = resolutions[camera][1];
1870
1871                /* setup device defaults */
1872                dev->overlay.w.left = 150;
1873                dev->overlay.w.top = 50;
1874                dev->overlay.w.width = 1024;
1875                dev->overlay.w.height = 768;
1876                dev->overlay.clipcount = 0;
1877                dev->overlay.field = V4L2_FIELD_NONE;
1878                dev->overlay.global_alpha = 255;
1879
1880                dev->capture.fmt = &formats[3]; /* JPEG */
1881
1882                /* v4l device registration */
1883                snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name),
1884                         "%s", BM2835_MMAL_MODULE_NAME);
1885                ret = v4l2_device_register(NULL, &dev->v4l2_dev);
1886                if (ret)
1887                        goto free_dev;
1888
1889                /* setup v4l controls */
1890                ret = bm2835_mmal_init_controls(dev, &dev->ctrl_handler);
1891                if (ret < 0)
1892                        goto unreg_dev;
1893                dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler;
1894
1895                /* mmal init */
1896                dev->instance = instance;
1897                ret = mmal_init(dev);
1898                if (ret < 0)
1899                        goto unreg_dev;
1900
1901                /* initialize queue */
1902                q = &dev->capture.vb_vidq;
1903                memset(q, 0, sizeof(*q));
1904                q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1905                q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
1906                q->drv_priv = dev;
1907                q->buf_struct_size = sizeof(struct mmal_buffer);
1908                q->ops = &bm2835_mmal_video_qops;
1909                q->mem_ops = &vb2_vmalloc_memops;
1910                q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1911                ret = vb2_queue_init(q);
1912                if (ret < 0)
1913                        goto unreg_dev;
1914
1915                /* v4l2 core mutex used to protect all fops and v4l2 ioctls. */
1916                mutex_init(&dev->mutex);
1917
1918                /* initialise video devices */
1919                ret = bm2835_mmal_init_device(dev, &dev->vdev);
1920                if (ret < 0)
1921                        goto unreg_dev;
1922
1923                /* Really want to call vidioc_s_fmt_vid_cap with the default
1924                 * format, but currently the APIs don't join up.
1925                 */
1926                ret = mmal_setup_components(dev, &default_v4l2_format);
1927                if (ret < 0) {
1928                        v4l2_err(&dev->v4l2_dev,
1929                                 "%s: could not setup components\n", __func__);
1930                        goto unreg_dev;
1931                }
1932
1933                v4l2_info(&dev->v4l2_dev,
1934                          "Broadcom 2835 MMAL video capture ver %s loaded.\n",
1935                          BM2835_MMAL_VERSION);
1936
1937                gdev[camera] = dev;
1938        }
1939        return 0;
1940
1941unreg_dev:
1942        v4l2_ctrl_handler_free(&dev->ctrl_handler);
1943        v4l2_device_unregister(&dev->v4l2_dev);
1944
1945free_dev:
1946        kfree(dev);
1947
1948cleanup_gdev:
1949        for (i = 0; i < camera; i++) {
1950                bcm2835_cleanup_instance(gdev[i]);
1951                gdev[i] = NULL;
1952        }
1953        pr_info("%s: error %d while loading driver\n",
1954                BM2835_MMAL_MODULE_NAME, ret);
1955
1956        return ret;
1957}
1958
1959static int bcm2835_mmal_remove(struct platform_device *pdev)
1960{
1961        int camera;
1962        struct vchiq_mmal_instance *instance = gdev[0]->instance;
1963
1964        for (camera = 0; camera < MAX_BCM2835_CAMERAS; camera++) {
1965                bcm2835_cleanup_instance(gdev[camera]);
1966                gdev[camera] = NULL;
1967        }
1968        vchiq_mmal_finalise(instance);
1969
1970        return 0;
1971}
1972
1973static struct platform_driver bcm2835_camera_driver = {
1974        .probe          = bcm2835_mmal_probe,
1975        .remove         = bcm2835_mmal_remove,
1976        .driver         = {
1977                .name   = "bcm2835-camera",
1978        },
1979};
1980
1981module_platform_driver(bcm2835_camera_driver)
1982