target/linux/brcm2708/patches-4.4/0233-V4L2-driver-updates-1393.patch

   1 From 21e3542368e5df2eecb87f32187cb7a64c55fd04 Mon Sep 17 00:00:00 2001
   2 From: 6by9 <6by9@users.noreply.github.com>
   3 Date: Fri, 8 Apr 2016 18:15:43 +0100
   4 Subject: [PATCH] V4L2 driver updates (#1393)
   5
   6 * BCM2835-V4L2: Correct ISO control and add V4L2_CID_ISO_SENSITIVITY_AUTO
   7
   8 https://github.com/raspberrypi/linux/issues/1251
   9
  10 V4L2_CID_ISO_SENSITIVITY was not advertising ISO*1000 as it should.
  11 V4L2_CID_ISO_SENSITIVITY_AUTO was not implemented, so was taking
  12 V4L2_CID_ISO_SENSITIVITY as 0 for auto mode.
  13 Still accepts 0 for auto, but also abides by the new parameter.
  14
  15 Signed-off-by: Dave Stevenson <6by9@users.noreply.github.com>
  16
  17 * BCM2835-V4L2: Add a video_nr parameter.
  18
  19 Adds a kernel parameter "video_nr" to specify the preferred
  20 /dev/videoX device node.
  21 https://www.raspberrypi.org/forums/viewtopic.php?f=38&t=136120&p=905545
  22
  23 Signed-off-by: Dave Stevenson <6by9@users.noreply.github.com>
  24
  25 * BCM2835-V4L2: Add support for multiple cameras
  26
  27 Ask GPU on load how many cameras have been detected, and
  28 enumerate that number of devices.
  29 Only applicable on the Compute Module as no other device
  30 exposes multiple CSI2 interfaces.
  31
  32 Signed-off-by: Dave Stevenson <6by9@users.noreply.github.com>
  33
  34 * BCM2835-V4L2: Add control of the overlay location and alpha.
  35
  36 Actually do something useful in vidioc_s_fmt_vid_overlay and
  37 vidioc_try_fmt_vid_overlay, rather than effectively having
  38 read-only fields.
  39
  40 Signed-off-by: Dave Stevenson <6by9@users.noreply.github.com>
  41
  42 * BCM2835-V4L2: V4L2-Compliance failure fix
  43
  44 VIDIOC_TRY_FMT was failing due to bytesperline not
  45 being set correctly by default.
  46
  47 Signed-off-by: Dave Stevenson <6by9@users.noreply.github.com>
  48
  49 * BCM2835-V4L2: Make all module parameters static
  50
  51 Clean up to correct variable scope
  52
  53 Signed-off-by: Dave Stevenson <6by9@users.noreply.github.com>
  54 ---
  55  drivers/media/platform/bcm2835/bcm2835-camera.c  | 372 +++++++++++++++--------
  56  drivers/media/platform/bcm2835/bcm2835-camera.h  |  19 +-
  57  drivers/media/platform/bcm2835/controls.c        |  31 +-
  58  drivers/media/platform/bcm2835/mmal-parameters.h |  33 ++
  59  4 files changed, 320 insertions(+), 135 deletions(-)
  60
  61 --- a/drivers/media/platform/bcm2835/bcm2835-camera.c
  62 +++ b/drivers/media/platform/bcm2835/bcm2835-camera.c
  63 @@ -45,6 +45,8 @@
  64  #define MAX_VIDEO_MODE_WIDTH 1280
  65  #define MAX_VIDEO_MODE_HEIGHT 720
  66
  67 +#define MAX_BCM2835_CAMERAS 2
  68 +
  69  MODULE_DESCRIPTION("Broadcom 2835 MMAL video capture");
  70  MODULE_AUTHOR("Vincent Sanders");
  71  MODULE_LICENSE("GPL");
  72 @@ -54,8 +56,13 @@ int bcm2835_v4l2_debug;
  73  module_param_named(debug, bcm2835_v4l2_debug, int, 0644);
  74  MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2");
  75
  76 -int max_video_width = MAX_VIDEO_MODE_WIDTH;
  77 -int max_video_height = MAX_VIDEO_MODE_HEIGHT;
  78 +#define UNSET (-1)
  79 +static int video_nr[] = {[0 ... (MAX_BCM2835_CAMERAS - 1)] = UNSET };
  80 +module_param_array(video_nr, int, NULL, 0644);
  81 +MODULE_PARM_DESC(video_nr, "videoX start numbers, -1 is autodetect");
  82 +
  83 +static int max_video_width = MAX_VIDEO_MODE_WIDTH;
  84 +static int max_video_height = MAX_VIDEO_MODE_HEIGHT;
  85  module_param(max_video_width, int, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
  86  MODULE_PARM_DESC(max_video_width, "Threshold for video mode");
  87  module_param(max_video_height, int, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
  88 @@ -70,11 +77,12 @@ MODULE_PARM_DESC(max_video_height, "Thre
  89   * our function table list (actually switch to an alternate set, but same
  90   * result).
  91   */
  92 -int gst_v4l2src_is_broken = 0;
  93 +static int gst_v4l2src_is_broken;
  94  module_param(gst_v4l2src_is_broken, int, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
  95  MODULE_PARM_DESC(gst_v4l2src_is_broken, "If non-zero, enable workaround for Gstreamer");
  96
  97 -static struct bm2835_mmal_dev *gdev;   /* global device data */
  98 +/* global device data array */
  99 +static struct bm2835_mmal_dev *gdev[MAX_BCM2835_CAMERAS];
 100
 101  #define FPS_MIN 1
 102  #define FPS_MAX 90
 103 @@ -413,6 +421,17 @@ static int enable_camera(struct bm2835_m
 104  {
 105         int ret;
 106         if (!dev->camera_use_count) {
 107 +               ret = vchiq_mmal_port_parameter_set(
 108 +                       dev->instance,
 109 +                       &dev->component[MMAL_COMPONENT_CAMERA]->control,
 110 +                       MMAL_PARAMETER_CAMERA_NUM, &dev->camera_num,
 111 +                       sizeof(dev->camera_num));
 112 +               if (ret < 0) {
 113 +                       v4l2_err(&dev->v4l2_dev,
 114 +                                "Failed setting camera num, ret %d\n", ret);
 115 +                       return -EINVAL;
 116 +               }
 117 +
 118                 ret = vchiq_mmal_component_enable(
 119                                 dev->instance,
 120                                 dev->component[MMAL_COMPONENT_CAMERA]);
 121 @@ -647,6 +666,30 @@ static struct vb2_ops bm2835_mmal_video_
 122         IOCTL operations
 123     ------------------------------------------------------------------*/
 124
 125 +static int set_overlay_params(struct bm2835_mmal_dev *dev,
 126 +                             struct vchiq_mmal_port *port)
 127 +{
 128 +       int ret;
 129 +       struct mmal_parameter_displayregion prev_config = {
 130 +       .set = MMAL_DISPLAY_SET_LAYER | MMAL_DISPLAY_SET_ALPHA |
 131 +           MMAL_DISPLAY_SET_DEST_RECT | MMAL_DISPLAY_SET_FULLSCREEN,
 132 +       .layer = PREVIEW_LAYER,
 133 +       .alpha = dev->overlay.global_alpha,
 134 +       .fullscreen = 0,
 135 +       .dest_rect = {
 136 +                     .x = dev->overlay.w.left,
 137 +                     .y = dev->overlay.w.top,
 138 +                     .width = dev->overlay.w.width,
 139 +                     .height = dev->overlay.w.height,
 140 +                     },
 141 +       };
 142 +       ret = vchiq_mmal_port_parameter_set(dev->instance, port,
 143 +                                           MMAL_PARAMETER_DISPLAYREGION,
 144 +                                           &prev_config, sizeof(prev_config));
 145 +
 146 +       return ret;
 147 +}
 148 +
 149  /* overlay ioctl */
 150  static int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv,
 151                                        struct v4l2_fmtdesc *f)
 152 @@ -678,10 +721,31 @@ static int vidioc_g_fmt_vid_overlay(stru
 153  static int vidioc_try_fmt_vid_overlay(struct file *file, void *priv,
 154                                       struct v4l2_format *f)
 155  {
 156 -       /* Only support one format so get the current one. */
 157 -       vidioc_g_fmt_vid_overlay(file, priv, f);
 158 +       struct bm2835_mmal_dev *dev = video_drvdata(file);
 159
 160 -       /* todo: allow the size and/or offset to be changed. */
 161 +       f->fmt.win.field = V4L2_FIELD_NONE;
 162 +       f->fmt.win.chromakey = 0;
 163 +       f->fmt.win.clips = NULL;
 164 +       f->fmt.win.clipcount = 0;
 165 +       f->fmt.win.bitmap = NULL;
 166 +
 167 +       v4l_bound_align_image(&f->fmt.win.w.width, MIN_WIDTH, MAX_WIDTH, 1,
 168 +                             &f->fmt.win.w.height, MIN_HEIGHT, MAX_HEIGHT,
 169 +                             1, 0);
 170 +       v4l_bound_align_image(&f->fmt.win.w.left, MIN_WIDTH, MAX_WIDTH, 1,
 171 +                             &f->fmt.win.w.top, MIN_HEIGHT, MAX_HEIGHT,
 172 +                             1, 0);
 173 +
 174 +       v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 175 +               "Overlay: Now w/h %dx%d l/t %dx%d\n",
 176 +               f->fmt.win.w.width, f->fmt.win.w.height,
 177 +               f->fmt.win.w.left, f->fmt.win.w.top);
 178 +
 179 +       v4l2_dump_win_format(1,
 180 +                            bcm2835_v4l2_debug,
 181 +                            &dev->v4l2_dev,
 182 +                            &f->fmt.win,
 183 +                            __func__);
 184         return 0;
 185  }
 186
 187 @@ -693,8 +757,11 @@ static int vidioc_s_fmt_vid_overlay(stru
 188         vidioc_try_fmt_vid_overlay(file, priv, f);
 189
 190         dev->overlay = f->fmt.win;
 191 +       if (dev->component[MMAL_COMPONENT_PREVIEW]->enabled) {
 192 +               set_overlay_params(dev,
 193 +                       &dev->component[MMAL_COMPONENT_PREVIEW]->input[0]);
 194 +       }
 195
 196 -       /* todo: program the preview port parameters */
 197         return 0;
 198  }
 199
 200 @@ -704,20 +771,6 @@ static int vidioc_overlay(struct file *f
 201         struct bm2835_mmal_dev *dev = video_drvdata(file);
 202         struct vchiq_mmal_port *src;
 203         struct vchiq_mmal_port *dst;
 204 -       struct mmal_parameter_displayregion prev_config = {
 205 -               .set = MMAL_DISPLAY_SET_LAYER | MMAL_DISPLAY_SET_ALPHA |
 206 -                   MMAL_DISPLAY_SET_DEST_RECT | MMAL_DISPLAY_SET_FULLSCREEN,
 207 -               .layer = PREVIEW_LAYER,
 208 -               .alpha = 255,
 209 -               .fullscreen = 0,
 210 -               .dest_rect = {
 211 -                             .x = dev->overlay.w.left,
 212 -                             .y = dev->overlay.w.top,
 213 -                             .width = dev->overlay.w.width,
 214 -                             .height = dev->overlay.w.height,
 215 -                             },
 216 -       };
 217 -
 218         if ((on && dev->component[MMAL_COMPONENT_PREVIEW]->enabled) ||
 219             (!on && !dev->component[MMAL_COMPONENT_PREVIEW]->enabled))
 220                 return 0;       /* already in requested state */
 221 @@ -749,9 +802,7 @@ static int vidioc_overlay(struct file *f
 222         if (ret < 0)
 223                 goto error;
 224
 225 -       ret = vchiq_mmal_port_parameter_set(dev->instance, dst,
 226 -                                           MMAL_PARAMETER_DISPLAYREGION,
 227 -                                           &prev_config, sizeof(prev_config));
 228 +       ret = set_overlay_params(dev, dst);
 229         if (ret < 0)
 230                 goto error;
 231
 232 @@ -782,6 +833,9 @@ static int vidioc_g_fbuf(struct file *fi
 233         struct vchiq_mmal_port *preview_port =
 234                     &dev->component[MMAL_COMPONENT_CAMERA]->
 235                     output[MMAL_CAMERA_PORT_PREVIEW];
 236 +
 237 +       a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
 238 +                       V4L2_FBUF_CAP_GLOBAL_ALPHA;
 239         a->flags = V4L2_FBUF_FLAG_OVERLAY;
 240         a->fmt.width = preview_port->es.video.width;
 241         a->fmt.height = preview_port->es.video.height;
 242 @@ -1445,6 +1499,34 @@ static struct video_device vdev_template
 243         .release = video_device_release_empty,
 244  };
 245
 246 +static int get_num_cameras(struct vchiq_mmal_instance *instance)
 247 +{
 248 +       int ret;
 249 +       struct vchiq_mmal_component  *cam_info_component;
 250 +       struct mmal_parameter_camera_info_t cam_info = {0};
 251 +       int param_size = sizeof(cam_info);
 252 +
 253 +       /* create a camera_info component */
 254 +       ret = vchiq_mmal_component_init(instance, "camera_info",
 255 +                                       &cam_info_component);
 256 +       if (ret < 0)
 257 +               /* Unusual failure - let's guess one camera. */
 258 +               return 1;
 259 +
 260 +       if (vchiq_mmal_port_parameter_get(instance,
 261 +                                         &cam_info_component->control,
 262 +                                         MMAL_PARAMETER_CAMERA_INFO,
 263 +                                         &cam_info,
 264 +                                         &param_size)) {
 265 +               pr_info("Failed to get camera info\n");
 266 +       }
 267 +
 268 +       vchiq_mmal_component_finalise(instance,
 269 +                                     cam_info_component);
 270 +
 271 +       return cam_info.num_cameras;
 272 +}
 273 +
 274  static int set_camera_parameters(struct vchiq_mmal_instance *instance,
 275                                  struct vchiq_mmal_component *camera)
 276  {
 277 @@ -1685,7 +1767,9 @@ static int __init bm2835_mmal_init_devic
 278         /* video device needs to be able to access instance data */
 279         video_set_drvdata(vfd, dev);
 280
 281 -       ret = video_register_device(vfd, VFL_TYPE_GRABBER, -1);
 282 +       ret = video_register_device(vfd,
 283 +                                   VFL_TYPE_GRABBER,
 284 +                                   video_nr[dev->camera_num]);
 285         if (ret < 0)
 286                 return ret;
 287
 288 @@ -1696,10 +1780,52 @@ static int __init bm2835_mmal_init_devic
 289         return 0;
 290  }
 291
 292 +void bcm2835_cleanup_instance(struct bm2835_mmal_dev *dev)
 293 +{
 294 +       if (!dev)
 295 +               return;
 296 +
 297 +       v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
 298 +                 video_device_node_name(&dev->vdev));
 299 +
 300 +       video_unregister_device(&dev->vdev);
 301 +
 302 +       if (dev->capture.encode_component) {
 303 +               v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 304 +                        "mmal_exit - disconnect tunnel\n");
 305 +               vchiq_mmal_port_connect_tunnel(dev->instance,
 306 +                                              dev->capture.camera_port, NULL);
 307 +               vchiq_mmal_component_disable(dev->instance,
 308 +                                            dev->capture.encode_component);
 309 +       }
 310 +       vchiq_mmal_component_disable(dev->instance,
 311 +                                    dev->component[MMAL_COMPONENT_CAMERA]);
 312 +
 313 +       vchiq_mmal_component_finalise(dev->instance,
 314 +                                     dev->
 315 +                                     component[MMAL_COMPONENT_VIDEO_ENCODE]);
 316 +
 317 +       vchiq_mmal_component_finalise(dev->instance,
 318 +                                     dev->
 319 +                                     component[MMAL_COMPONENT_IMAGE_ENCODE]);
 320 +
 321 +       vchiq_mmal_component_finalise(dev->instance,
 322 +                                     dev->component[MMAL_COMPONENT_PREVIEW]);
 323 +
 324 +       vchiq_mmal_component_finalise(dev->instance,
 325 +                                     dev->component[MMAL_COMPONENT_CAMERA]);
 326 +
 327 +       v4l2_ctrl_handler_free(&dev->ctrl_handler);
 328 +
 329 +       v4l2_device_unregister(&dev->v4l2_dev);
 330 +
 331 +       kfree(dev);
 332 +}
 333 +
 334  static struct v4l2_format default_v4l2_format = {
 335         .fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG,
 336         .fmt.pix.width = 1024,
 337 -       .fmt.pix.bytesperline = 1024,
 338 +       .fmt.pix.bytesperline = 0,
 339         .fmt.pix.height = 768,
 340         .fmt.pix.sizeimage = 1024*768,
 341  };
 342 @@ -1709,76 +1835,93 @@ static int __init bm2835_mmal_init(void)
 343         int ret;
 344         struct bm2835_mmal_dev *dev;
 345         struct vb2_queue *q;
 346 +       int camera;
 347 +       unsigned int num_cameras;
 348 +       struct vchiq_mmal_instance *instance;
 349
 350 -       dev = kzalloc(sizeof(*gdev), GFP_KERNEL);
 351 -       if (!dev)
 352 -               return -ENOMEM;
 353 -
 354 -       /* setup device defaults */
 355 -       dev->overlay.w.left = 150;
 356 -       dev->overlay.w.top = 50;
 357 -       dev->overlay.w.width = 1024;
 358 -       dev->overlay.w.height = 768;
 359 -       dev->overlay.clipcount = 0;
 360 -       dev->overlay.field = V4L2_FIELD_NONE;
 361 -
 362 -       dev->capture.fmt = &formats[3]; /* JPEG */
 363 -
 364 -       /* v4l device registration */
 365 -       snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name),
 366 -                "%s", BM2835_MMAL_MODULE_NAME);
 367 -       ret = v4l2_device_register(NULL, &dev->v4l2_dev);
 368 -       if (ret)
 369 -               goto free_dev;
 370 -
 371 -       /* setup v4l controls */
 372 -       ret = bm2835_mmal_init_controls(dev, &dev->ctrl_handler);
 373 -       if (ret < 0)
 374 -               goto unreg_dev;
 375 -       dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler;
 376 -
 377 -       /* mmal init */
 378 -       ret = mmal_init(dev);
 379 +       ret = vchiq_mmal_init(&instance);
 380         if (ret < 0)
 381 -               goto unreg_dev;
 382 +               return ret;
 383
 384 -       /* initialize queue */
 385 -       q = &dev->capture.vb_vidq;
 386 -       memset(q, 0, sizeof(*q));
 387 -       q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 388 -       q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
 389 -       q->drv_priv = dev;
 390 -       q->buf_struct_size = sizeof(struct mmal_buffer);
 391 -       q->ops = &bm2835_mmal_video_qops;
 392 -       q->mem_ops = &vb2_vmalloc_memops;
 393 -       q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 394 -       ret = vb2_queue_init(q);
 395 -       if (ret < 0)
 396 -               goto unreg_dev;
 397 +       num_cameras = get_num_cameras(instance);
 398 +       if (num_cameras > MAX_BCM2835_CAMERAS)
 399 +               num_cameras = MAX_BCM2835_CAMERAS;
 400 +
 401 +       for (camera = 0; camera < num_cameras; camera++) {
 402 +               dev = kzalloc(sizeof(struct bm2835_mmal_dev), GFP_KERNEL);
 403 +               if (!dev)
 404 +                       return -ENOMEM;
 405 +
 406 +               dev->camera_num = camera;
 407 +
 408 +               /* setup device defaults */
 409 +               dev->overlay.w.left = 150;
 410 +               dev->overlay.w.top = 50;
 411 +               dev->overlay.w.width = 1024;
 412 +               dev->overlay.w.height = 768;
 413 +               dev->overlay.clipcount = 0;
 414 +               dev->overlay.field = V4L2_FIELD_NONE;
 415 +               dev->overlay.global_alpha = 255;
 416 +
 417 +               dev->capture.fmt = &formats[3]; /* JPEG */
 418 +
 419 +               /* v4l device registration */
 420 +               snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name),
 421 +                        "%s", BM2835_MMAL_MODULE_NAME);
 422 +               ret = v4l2_device_register(NULL, &dev->v4l2_dev);
 423 +               if (ret)
 424 +                       goto free_dev;
 425
 426 -       /* v4l2 core mutex used to protect all fops and v4l2 ioctls. */
 427 -       mutex_init(&dev->mutex);
 428 +               /* setup v4l controls */
 429 +               ret = bm2835_mmal_init_controls(dev, &dev->ctrl_handler);
 430 +               if (ret < 0)
 431 +                       goto unreg_dev;
 432 +               dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler;
 433 +
 434 +               /* mmal init */
 435 +               dev->instance = instance;
 436 +               ret = mmal_init(dev);
 437 +               if (ret < 0)
 438 +                       goto unreg_dev;
 439 +
 440 +               /* initialize queue */
 441 +               q = &dev->capture.vb_vidq;
 442 +               memset(q, 0, sizeof(*q));
 443 +               q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 444 +               q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
 445 +               q->drv_priv = dev;
 446 +               q->buf_struct_size = sizeof(struct mmal_buffer);
 447 +               q->ops = &bm2835_mmal_video_qops;
 448 +               q->mem_ops = &vb2_vmalloc_memops;
 449 +               q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 450 +               ret = vb2_queue_init(q);
 451 +               if (ret < 0)
 452 +                       goto unreg_dev;
 453 +
 454 +               /* v4l2 core mutex used to protect all fops and v4l2 ioctls. */
 455 +               mutex_init(&dev->mutex);
 456 +
 457 +               /* initialise video devices */
 458 +               ret = bm2835_mmal_init_device(dev, &dev->vdev);
 459 +               if (ret < 0)
 460 +                       goto unreg_dev;
 461 +
 462 +               /* Really want to call vidioc_s_fmt_vid_cap with the default
 463 +                * format, but currently the APIs don't join up.
 464 +                */
 465 +               ret = mmal_setup_components(dev, &default_v4l2_format);
 466 +               if (ret < 0) {
 467 +                       v4l2_err(&dev->v4l2_dev,
 468 +                                "%s: could not setup components\n", __func__);
 469 +                       goto unreg_dev;
 470 +               }
 471
 472 -       /* initialise video devices */
 473 -       ret = bm2835_mmal_init_device(dev, &dev->vdev);
 474 -       if (ret < 0)
 475 -               goto unreg_dev;
 476 +               v4l2_info(&dev->v4l2_dev,
 477 +                         "Broadcom 2835 MMAL video capture ver %s loaded.\n",
 478 +                         BM2835_MMAL_VERSION);
 479
 480 -       /* Really want to call vidioc_s_fmt_vid_cap with the default
 481 -        * format, but currently the APIs don't join up.
 482 -        */
 483 -       ret = mmal_setup_components(dev, &default_v4l2_format);
 484 -       if (ret < 0) {
 485 -               v4l2_err(&dev->v4l2_dev,
 486 -                        "%s: could not setup components\n", __func__);
 487 -               goto unreg_dev;
 488 +               gdev[camera] = dev;
 489         }
 490 -
 491 -       v4l2_info(&dev->v4l2_dev,
 492 -                 "Broadcom 2835 MMAL video capture ver %s loaded.\n",
 493 -                 BM2835_MMAL_VERSION);
 494 -
 495 -       gdev = dev;
 496         return 0;
 497
 498  unreg_dev:
 499 @@ -1788,8 +1931,11 @@ unreg_dev:
 500  free_dev:
 501         kfree(dev);
 502
 503 -       v4l2_err(&dev->v4l2_dev,
 504 -                "%s: error %d while loading driver\n",
 505 +       for ( ; camera > 0; camera--) {
 506 +               bcm2835_cleanup_instance(gdev[camera]);
 507 +               gdev[camera] = NULL;
 508 +       }
 509 +       pr_info("%s: error %d while loading driver\n",
 510                  BM2835_MMAL_MODULE_NAME, ret);
 511
 512         return ret;
 513 @@ -1797,46 +1943,14 @@ free_dev:
 514
 515  static void __exit bm2835_mmal_exit(void)
 516  {
 517 -       if (!gdev)
 518 -               return;
 519 -
 520 -       v4l2_info(&gdev->v4l2_dev, "unregistering %s\n",
 521 -                 video_device_node_name(&gdev->vdev));
 522 +       int camera;
 523 +       struct vchiq_mmal_instance *instance = gdev[0]->instance;
 524
 525 -       video_unregister_device(&gdev->vdev);
 526 -
 527 -       if (gdev->capture.encode_component) {
 528 -               v4l2_dbg(1, bcm2835_v4l2_debug, &gdev->v4l2_dev,
 529 -                        "mmal_exit - disconnect tunnel\n");
 530 -               vchiq_mmal_port_connect_tunnel(gdev->instance,
 531 -                                              gdev->capture.camera_port, NULL);
 532 -               vchiq_mmal_component_disable(gdev->instance,
 533 -                                            gdev->capture.encode_component);
 534 +       for (camera = 0; camera < MAX_BCM2835_CAMERAS; camera++) {
 535 +               bcm2835_cleanup_instance(gdev[camera]);
 536 +               gdev[camera] = NULL;
 537         }
 538 -       vchiq_mmal_component_disable(gdev->instance,
 539 -                                    gdev->component[MMAL_COMPONENT_CAMERA]);
 540 -
 541 -       vchiq_mmal_component_finalise(gdev->instance,
 542 -                                     gdev->
 543 -                                     component[MMAL_COMPONENT_VIDEO_ENCODE]);
 544 -
 545 -       vchiq_mmal_component_finalise(gdev->instance,
 546 -                                     gdev->
 547 -                                     component[MMAL_COMPONENT_IMAGE_ENCODE]);
 548 -
 549 -       vchiq_mmal_component_finalise(gdev->instance,
 550 -                                     gdev->component[MMAL_COMPONENT_PREVIEW]);
 551 -
 552 -       vchiq_mmal_component_finalise(gdev->instance,
 553 -                                     gdev->component[MMAL_COMPONENT_CAMERA]);
 554 -
 555 -       vchiq_mmal_finalise(gdev->instance);
 556 -
 557 -       v4l2_ctrl_handler_free(&gdev->ctrl_handler);
 558 -
 559 -       v4l2_device_unregister(&gdev->v4l2_dev);
 560 -
 561 -       kfree(gdev);
 562 +       vchiq_mmal_finalise(instance);
 563  }
 564
 565  module_init(bm2835_mmal_init);
 566 --- a/drivers/media/platform/bcm2835/bcm2835-camera.h
 567 +++ b/drivers/media/platform/bcm2835/bcm2835-camera.h
 568 @@ -15,7 +15,7 @@
 569   * core driver device
 570   */
 571
 572 -#define V4L2_CTRL_COUNT 28 /* number of v4l controls */
 573 +#define V4L2_CTRL_COUNT 29 /* number of v4l controls */
 574
 575  enum {
 576         MMAL_COMPONENT_CAMERA = 0,
 577 @@ -58,6 +58,8 @@ struct bm2835_mmal_dev {
 578         enum mmal_parameter_exposuremeteringmode metering_mode;
 579         unsigned int              manual_shutter_speed;
 580         bool                      exp_auto_priority;
 581 +       bool manual_iso_enabled;
 582 +       uint32_t iso;
 583
 584         /* allocated mmal instance and components */
 585         struct vchiq_mmal_instance   *instance;
 586 @@ -104,6 +106,8 @@ struct bm2835_mmal_dev {
 587
 588         } capture;
 589
 590 +       unsigned int camera_num;
 591 +
 592  };
 593
 594  int bm2835_mmal_init_controls(
 595 @@ -124,3 +128,16 @@ int set_framerate_params(struct bm2835_m
 596                 (pix_fmt)->pixelformat, (pix_fmt)->bytesperline,        \
 597                 (pix_fmt)->sizeimage, (pix_fmt)->colorspace, (pix_fmt)->priv); \
 598  }
 599 +#define v4l2_dump_win_format(level, debug, dev, win_fmt, desc) \
 600 +{      \
 601 +       v4l2_dbg(level, debug, dev,     \
 602 +"%s: w %u h %u l %u t %u  field %u chromakey %06X clip %p " \
 603 +"clipcount %u bitmap %p\n", \
 604 +               desc == NULL ? "" : desc,       \
 605 +               (win_fmt)->w.width, (win_fmt)->w.height, \
 606 +               (win_fmt)->w.left, (win_fmt)->w.top, \
 607 +               (win_fmt)->field,       \
 608 +               (win_fmt)->chromakey,   \
 609 +               (win_fmt)->clips, (win_fmt)->clipcount, \
 610 +               (win_fmt)->bitmap); \
 611 +}
 612 --- a/drivers/media/platform/bcm2835/controls.c
 613 +++ b/drivers/media/platform/bcm2835/controls.c
 614 @@ -49,10 +49,13 @@ static const s64 ev_bias_qmenu[] = {
 615          4000
 616  };
 617
 618 -/* Supported ISO values
 619 +/* Supported ISO values (*1000)
 620   * ISOO = auto ISO
 621   */
 622  static const s64 iso_qmenu[] = {
 623 +       0, 100000, 200000, 400000, 800000,
 624 +};
 625 +static const uint32_t iso_values[] = {
 626         0, 100, 200, 400, 800,
 627  };
 628
 629 @@ -201,7 +204,7 @@ static int ctrl_set_value(struct bm2835_
 630                                              &u32_value, sizeof(u32_value));
 631  }
 632
 633 -static int ctrl_set_value_menu(struct bm2835_mmal_dev *dev,
 634 +static int ctrl_set_iso(struct bm2835_mmal_dev *dev,
 635                       struct v4l2_ctrl *ctrl,
 636                       const struct bm2835_mmal_v4l2_ctrl *mmal_ctrl)
 637  {
 638 @@ -211,12 +214,23 @@ static int ctrl_set_value_menu(struct bm
 639         if (ctrl->val > mmal_ctrl->max || ctrl->val < mmal_ctrl->min)
 640                 return 1;
 641
 642 +       if (ctrl->id == V4L2_CID_ISO_SENSITIVITY)
 643 +               dev->iso = iso_values[ctrl->val];
 644 +       else if (ctrl->id == V4L2_CID_ISO_SENSITIVITY_AUTO)
 645 +               dev->manual_iso_enabled =
 646 +                               (ctrl->val == V4L2_ISO_SENSITIVITY_MANUAL ?
 647 +                                                       true :
 648 +                                                       false);
 649 +
 650         control = &dev->component[MMAL_COMPONENT_CAMERA]->control;
 651
 652 -       u32_value = mmal_ctrl->imenu[ctrl->val];
 653 +       if (dev->manual_iso_enabled)
 654 +               u32_value = dev->iso;
 655 +       else
 656 +               u32_value = 0;
 657
 658         return vchiq_mmal_port_parameter_set(dev->instance, control,
 659 -                                            mmal_ctrl->mmal_id,
 660 +                                            MMAL_PARAMETER_ISO,
 661                                              &u32_value, sizeof(u32_value));
 662  }
 663
 664 @@ -956,7 +970,14 @@ static const struct bm2835_mmal_v4l2_ctr
 665                 V4L2_CID_ISO_SENSITIVITY, MMAL_CONTROL_TYPE_INT_MENU,
 666                 0, ARRAY_SIZE(iso_qmenu) - 1, 0, 1, iso_qmenu,
 667                 MMAL_PARAMETER_ISO,
 668 -               &ctrl_set_value_menu,
 669 +               &ctrl_set_iso,
 670 +               false
 671 +       },
 672 +       {
 673 +               V4L2_CID_ISO_SENSITIVITY_AUTO, MMAL_CONTROL_TYPE_STD_MENU,
 674 +               0, 1, V4L2_ISO_SENSITIVITY_AUTO, 1, NULL,
 675 +               MMAL_PARAMETER_ISO,
 676 +               &ctrl_set_iso,
 677                 false
 678         },
 679         {
 680 --- a/drivers/media/platform/bcm2835/mmal-parameters.h
 681 +++ b/drivers/media/platform/bcm2835/mmal-parameters.h
 682 @@ -654,3 +654,36 @@ struct mmal_parameter_imagefx_parameters
 683         u32 num_effect_params;
 684         u32 effect_parameter[MMAL_MAX_IMAGEFX_PARAMETERS];
 685  };
 686 +
 687 +#define MMAL_PARAMETER_CAMERA_INFO_MAX_CAMERAS 4
 688 +#define MMAL_PARAMETER_CAMERA_INFO_MAX_FLASHES 2
 689 +#define MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN 16
 690 +
 691 +struct mmal_parameter_camera_info_camera_t {
 692 +       u32    port_id;
 693 +       u32    max_width;
 694 +       u32    max_height;
 695 +       u32    lens_present;
 696 +       u8     camera_name[MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN];
 697 +};
 698 +
 699 +enum mmal_parameter_camera_info_flash_type_t {
 700 +       /* Make values explicit to ensure they match values in config ini */
 701 +       MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_XENON = 0,
 702 +       MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_LED   = 1,
 703 +       MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_OTHER = 2,
 704 +       MMAL_PARAMETER_CAMERA_INFO_FLASH_TYPE_MAX = 0x7FFFFFFF
 705 +};
 706 +
 707 +struct mmal_parameter_camera_info_flash_t {
 708 +       enum mmal_parameter_camera_info_flash_type_t flash_type;
 709 +};
 710 +
 711 +struct mmal_parameter_camera_info_t {
 712 +       u32                            num_cameras;
 713 +       u32                            num_flashes;
 714 +       struct mmal_parameter_camera_info_camera_t
 715 +                               cameras[MMAL_PARAMETER_CAMERA_INFO_MAX_CAMERAS];
 716 +       struct mmal_parameter_camera_info_flash_t
 717 +                               flashes[MMAL_PARAMETER_CAMERA_INFO_MAX_FLASHES];
 718 +};