target/linux/bcm27xx/patches-5.4/950-0681-staging-vc04_services-ISP-Add-a-more-complex-ISP-pro.patch

   1 From 05a5bc2bfa028885c844ccc2263029b5db9160b4 Mon Sep 17 00:00:00 2001
   2 From: Naushir Patuck <naush@raspberrypi.com>
   3 Date: Thu, 23 Apr 2020 10:17:37 +0100
   4 Subject: [PATCH] staging: vc04_services: ISP: Add a more complex ISP
   5  processing component
   6
   7 Driver for the BCM2835 ISP hardware block.  This driver uses the MMAL
   8 component to program the ISP hardware through the VC firmware.
   9
  10 The ISP component can produce two video stream outputs, and Bayer
  11 image statistics. This can't be encompassed in a simple V4L2
  12 M2M device, so create a new device that registers 4 video nodes.
  13
  14 Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
  15 ---
  16  MAINTAINERS                                   |    9 +
  17  drivers/staging/vc04_services/Kconfig         |    1 +
  18  drivers/staging/vc04_services/Makefile        |    1 +
  19  .../staging/vc04_services/bcm2835-isp/Kconfig |   14 +
  20  .../vc04_services/bcm2835-isp/Makefile        |    8 +
  21  .../bcm2835-isp/bcm2835-v4l2-isp.c            | 1627 +++++++++++++++++
  22  .../bcm2835-isp/bcm2835_isp_ctrls.h           |   67 +
  23  .../bcm2835-isp/bcm2835_isp_fmts.h            |  272 +++
  24  .../vc04_services/vchiq-mmal/mmal-encodings.h |    4 +
  25  .../vchiq-mmal/mmal-parameters.h              |  153 +-
  26  10 files changed, 2155 insertions(+), 1 deletion(-)
  27  create mode 100644 drivers/staging/vc04_services/bcm2835-isp/Kconfig
  28  create mode 100644 drivers/staging/vc04_services/bcm2835-isp/Makefile
  29  create mode 100644 drivers/staging/vc04_services/bcm2835-isp/bcm2835-v4l2-isp.c
  30  create mode 100644 drivers/staging/vc04_services/bcm2835-isp/bcm2835_isp_ctrls.h
  31  create mode 100644 drivers/staging/vc04_services/bcm2835-isp/bcm2835_isp_fmts.h
  32
  33 --- a/MAINTAINERS
  34 +++ b/MAINTAINERS
  35 @@ -3212,6 +3212,15 @@ S:       Maintained
  36  F:     drivers/media/platform/bcm2835/
  37  F:     Documentation/devicetree/bindings/media/bcm2835-unicam.txt
  38
  39 +BROADCOM BCM2835 ISP DRIVER
  40 +M:     Raspberry Pi Kernel Maintenance <kernel-list@raspberrypi.com>
  41 +L:     linux-media@vger.kernel.org
  42 +S:     Maintained
  43 +F:     drivers/staging/vc04_services/bcm2835-isp
  44 +F:     include/uapi/linux/bcm2835-isp.h
  45 +F:     Documentation/media/v4l-drivers/bcm2835-isp.rst
  46 +F:     Documentation/media/uapi/v4l/pixfmt-meta-bcm2835-isp-stats.rst
  47 +
  48  BROADCOM BCM47XX MIPS ARCHITECTURE
  49  M:     Hauke Mehrtens <hauke@hauke-m.de>
  50  M:     Rafał Miłecki <zajec5@gmail.com>
  51 --- a/drivers/staging/vc04_services/Kconfig
  52 +++ b/drivers/staging/vc04_services/Kconfig
  53 @@ -25,6 +25,7 @@ source "drivers/staging/vc04_services/bc
  54  source "drivers/staging/vc04_services/vchiq-mmal/Kconfig"
  55  source "drivers/staging/vc04_services/vc-sm-cma/Kconfig"
  56  source "drivers/staging/vc04_services/bcm2835-codec/Kconfig"
  57 +source "drivers/staging/vc04_services/bcm2835-isp/Kconfig"
  58
  59  endif
  60
  61 --- a/drivers/staging/vc04_services/Makefile
  62 +++ b/drivers/staging/vc04_services/Makefile
  63 @@ -15,6 +15,7 @@ obj-$(CONFIG_VIDEO_BCM2835)           += bcm2835-
  64  obj-$(CONFIG_BCM2835_VCHIQ_MMAL)       += vchiq-mmal/
  65  obj-$(CONFIG_BCM_VC_SM_CMA)            += vc-sm-cma/
  66  obj-$(CONFIG_VIDEO_CODEC_BCM2835)      += bcm2835-codec/
  67 +obj-$(CONFIG_VIDEO_ISP_BCM2835)                += bcm2835-isp/
  68
  69  ccflags-y += -Idrivers/staging/vc04_services -D__VCCOREVER__=0x04000000
  70
  71 --- /dev/null
  72 +++ b/drivers/staging/vc04_services/bcm2835-isp/Kconfig
  73 @@ -0,0 +1,14 @@
  74 +config VIDEO_ISP_BCM2835
  75 +       tristate "BCM2835 ISP support"
  76 +       depends on MEDIA_SUPPORT
  77 +       depends on VIDEO_V4L2 && (ARCH_BCM2835 || COMPILE_TEST)
  78 +       depends on MEDIA_CONTROLLER
  79 +       select BCM2835_VCHIQ_MMAL
  80 +       select VIDEOBUF2_DMA_CONTIG
  81 +       help
  82 +         This is the V4L2 driver for the Broadcom BCM2835 ISP hardware.
  83 +         This operates over the VCHIQ interface to a service running on
  84 +         VideoCore.
  85 +
  86 +         To compile this driver as a module, choose M here: the module
  87 +         will be called bcm2835-isp.
  88 --- /dev/null
  89 +++ b/drivers/staging/vc04_services/bcm2835-isp/Makefile
  90 @@ -0,0 +1,8 @@
  91 +# SPDX-License-Identifier: GPL-2.0
  92 +bcm2835-isp-objs := bcm2835-v4l2-isp.o
  93 +
  94 +obj-$(CONFIG_VIDEO_ISP_BCM2835) += bcm2835-isp.o
  95 +
  96 +ccflags-y += \
  97 +       -I$(srctree)/drivers/staging/vc04_services \
  98 +       -D__VCCOREVER__=0x04000000
  99 --- /dev/null
 100 +++ b/drivers/staging/vc04_services/bcm2835-isp/bcm2835-v4l2-isp.c
 101 @@ -0,0 +1,1627 @@
 102 +// SPDX-License-Identifier: GPL-2.0
 103 +/*
 104 + * Broadcom BCM2835 ISP driver
 105 + *
 106 + * Copyright © 2019-2020 Raspberry Pi (Trading) Ltd.
 107 + *
 108 + * Author: Naushir Patuck (naush@raspberrypi.com)
 109 + *
 110 + */
 111 +
 112 +#include <linux/module.h>
 113 +#include <linux/platform_device.h>
 114 +
 115 +#include <media/v4l2-ctrls.h>
 116 +#include <media/v4l2-device.h>
 117 +#include <media/v4l2-event.h>
 118 +#include <media/v4l2-ioctl.h>
 119 +#include <media/videobuf2-dma-contig.h>
 120 +
 121 +#include "vchiq-mmal/mmal-msg.h"
 122 +#include "vchiq-mmal/mmal-parameters.h"
 123 +#include "vchiq-mmal/mmal-vchiq.h"
 124 +
 125 +#include "bcm2835_isp_ctrls.h"
 126 +#include "bcm2835_isp_fmts.h"
 127 +
 128 +static unsigned int debug;
 129 +module_param(debug, uint, 0644);
 130 +MODULE_PARM_DESC(debug, "activates debug info");
 131 +
 132 +static unsigned int video_nr = 13;
 133 +module_param(video_nr, uint, 0644);
 134 +MODULE_PARM_DESC(video_nr, "base video device number");
 135 +
 136 +#define BCM2835_ISP_NAME "bcm2835-isp"
 137 +#define BCM2835_ISP_ENTITY_NAME_LEN 32
 138 +
 139 +#define BCM2835_ISP_NUM_OUTPUTS 1
 140 +#define BCM2835_ISP_NUM_CAPTURES 2
 141 +#define BCM2835_ISP_NUM_METADATA 1
 142 +
 143 +#define BCM2835_ISP_NUM_NODES                                          \
 144 +               (BCM2835_ISP_NUM_OUTPUTS + BCM2835_ISP_NUM_CAPTURES +   \
 145 +                BCM2835_ISP_NUM_METADATA)
 146 +
 147 +/* Default frame dimension of 1280 pixels. */
 148 +#define DEFAULT_DIM 1280U
 149 +/*
 150 + * Maximum frame dimension of 16384 pixels.  Even though the ISP runs in tiles,
 151 + * have a sensible limit so that we do not create an excessive number of tiles
 152 + * to process.
 153 + */
 154 +#define MAX_DIM 16384U
 155 +/*
 156 + * Minimum frame dimension of 64 pixels.  Anything lower, and the tiling
 157 + * algorihtm may not be able to cope when applying filter context.
 158 + */
 159 +#define MIN_DIM 64U
 160 +
 161 +/* Per-queue, driver-specific private data */
 162 +struct bcm2835_isp_q_data {
 163 +       /*
 164 +        * These parameters should be treated as gospel, with everything else
 165 +        * being determined from them.
 166 +        */
 167 +       unsigned int bytesperline;
 168 +       unsigned int width;
 169 +       unsigned int height;
 170 +       unsigned int sizeimage;
 171 +       struct bcm2835_isp_fmt *fmt;
 172 +};
 173 +
 174 +/*
 175 + * Structure to describe a single node /dev/video<N> which represents a single
 176 + * input or output queue to the ISP device.
 177 + */
 178 +struct bcm2835_isp_node {
 179 +       int vfl_dir;
 180 +       unsigned int id;
 181 +       const char *name;
 182 +       struct video_device vfd;
 183 +       struct media_pad pad;
 184 +       struct media_intf_devnode *intf_devnode;
 185 +       struct media_link *intf_link;
 186 +       struct mutex lock; /* top level device node lock */
 187 +       struct mutex queue_lock;
 188 +
 189 +       struct vb2_queue queue;
 190 +       unsigned int sequence;
 191 +
 192 +       /* The list of formats supported on the node. */
 193 +       struct bcm2835_isp_fmt_list supported_fmts;
 194 +
 195 +       struct bcm2835_isp_q_data q_data;
 196 +
 197 +       /* Parent device structure */
 198 +       struct bcm2835_isp_dev *dev;
 199 +
 200 +       bool registered;
 201 +       bool media_node_registered;
 202 +       bool queue_init;
 203 +};
 204 +
 205 +/*
 206 + * Structure representing the entire ISP device, comprising several input and
 207 + * output nodes /dev/video<N>.
 208 + */
 209 +struct bcm2835_isp_dev {
 210 +       struct v4l2_device v4l2_dev;
 211 +       struct device *dev;
 212 +       struct v4l2_ctrl_handler ctrl_handler;
 213 +       struct media_device mdev;
 214 +       struct media_entity entity;
 215 +       bool media_device_registered;
 216 +       bool media_entity_registered;
 217 +       struct vchiq_mmal_instance *mmal_instance;
 218 +       struct vchiq_mmal_component *component;
 219 +       struct completion frame_cmplt;
 220 +
 221 +       struct bcm2835_isp_node node[BCM2835_ISP_NUM_NODES];
 222 +       struct media_pad pad[BCM2835_ISP_NUM_NODES];
 223 +       atomic_t num_streaming;
 224 +
 225 +       /* Image pipeline controls. */
 226 +       int r_gain;
 227 +       int b_gain;
 228 +};
 229 +
 230 +struct bcm2835_isp_buffer {
 231 +       struct vb2_v4l2_buffer vb;
 232 +       struct mmal_buffer mmal;
 233 +};
 234 +
 235 +static
 236 +inline struct bcm2835_isp_dev *node_get_dev(struct bcm2835_isp_node *node)
 237 +{
 238 +       return node->dev;
 239 +}
 240 +
 241 +static inline bool node_is_output(struct bcm2835_isp_node *node)
 242 +{
 243 +       return node->queue.type == V4L2_BUF_TYPE_VIDEO_OUTPUT;
 244 +}
 245 +
 246 +static inline bool node_is_capture(struct bcm2835_isp_node *node)
 247 +{
 248 +       return node->queue.type == V4L2_BUF_TYPE_VIDEO_CAPTURE;
 249 +}
 250 +
 251 +static inline bool node_is_stats(struct bcm2835_isp_node *node)
 252 +{
 253 +       return node->queue.type == V4L2_BUF_TYPE_META_CAPTURE;
 254 +}
 255 +
 256 +static inline enum v4l2_buf_type index_to_queue_type(int index)
 257 +{
 258 +       if (index < BCM2835_ISP_NUM_OUTPUTS)
 259 +               return V4L2_BUF_TYPE_VIDEO_OUTPUT;
 260 +       else if (index < BCM2835_ISP_NUM_OUTPUTS + BCM2835_ISP_NUM_CAPTURES)
 261 +               return V4L2_BUF_TYPE_VIDEO_CAPTURE;
 262 +       else
 263 +               return V4L2_BUF_TYPE_META_CAPTURE;
 264 +}
 265 +
 266 +static struct vchiq_mmal_port *get_port_data(struct bcm2835_isp_node *node)
 267 +{
 268 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 269 +
 270 +       if (!dev->component)
 271 +               return NULL;
 272 +
 273 +       switch (node->queue.type) {
 274 +       case V4L2_BUF_TYPE_VIDEO_OUTPUT:
 275 +               return &dev->component->input[node->id];
 276 +       case V4L2_BUF_TYPE_VIDEO_CAPTURE:
 277 +       case V4L2_BUF_TYPE_META_CAPTURE:
 278 +               return &dev->component->output[node->id];
 279 +       default:
 280 +               v4l2_err(&dev->v4l2_dev, "%s: Invalid queue type %u\n",
 281 +                        __func__, node->queue.type);
 282 +               break;
 283 +       }
 284 +       return NULL;
 285 +}
 286 +
 287 +static int set_isp_param(struct bcm2835_isp_node *node, u32 parameter,
 288 +                        void *value, u32 value_size)
 289 +{
 290 +       struct vchiq_mmal_port *port = get_port_data(node);
 291 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 292 +
 293 +       return vchiq_mmal_port_parameter_set(dev->mmal_instance, port,
 294 +                                            parameter, value, value_size);
 295 +}
 296 +
 297 +static int set_wb_gains(struct bcm2835_isp_node *node)
 298 +{
 299 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 300 +       struct mmal_parameter_awbgains gains = {
 301 +               .r_gain = { dev->r_gain, 1000 },
 302 +               .b_gain = { dev->b_gain, 1000 }
 303 +       };
 304 +
 305 +       return set_isp_param(node, MMAL_PARAMETER_CUSTOM_AWB_GAINS,
 306 +                            &gains, sizeof(gains));
 307 +}
 308 +
 309 +static int set_digital_gain(struct bcm2835_isp_node *node, uint32_t gain)
 310 +{
 311 +       struct mmal_parameter_rational digital_gain = {
 312 +               .num = gain,
 313 +               .den = 1000
 314 +       };
 315 +
 316 +       return set_isp_param(node, MMAL_PARAMETER_DIGITAL_GAIN,
 317 +                            &digital_gain, sizeof(digital_gain));
 318 +}
 319 +
 320 +static const struct bcm2835_isp_fmt *get_fmt(u32 mmal_fmt)
 321 +{
 322 +       unsigned int i;
 323 +
 324 +       for (i = 0; i < ARRAY_SIZE(supported_formats); i++) {
 325 +               if (supported_formats[i].mmal_fmt == mmal_fmt)
 326 +                       return &supported_formats[i];
 327 +       }
 328 +       return NULL;
 329 +}
 330 +
 331 +static struct bcm2835_isp_fmt *find_format(struct v4l2_format *f,
 332 +                                          struct bcm2835_isp_node *node)
 333 +{
 334 +       struct bcm2835_isp_fmt_list *fmts = &node->supported_fmts;
 335 +       struct bcm2835_isp_fmt *fmt;
 336 +       unsigned int i;
 337 +
 338 +       for (i = 0; i < fmts->num_entries; i++) {
 339 +               fmt = &fmts->list[i];
 340 +               if (fmt->fourcc == (node_is_stats(node) ?
 341 +                                           f->fmt.meta.dataformat :
 342 +                                           f->fmt.pix.pixelformat))
 343 +                       return fmt;
 344 +       }
 345 +
 346 +       return NULL;
 347 +}
 348 +
 349 +/* vb2_to_mmal_buffer() - converts vb2 buffer header to MMAL
 350 + *
 351 + * Copies all the required fields from a VB2 buffer to the MMAL buffer header,
 352 + * ready for sending to the VPU.
 353 + */
 354 +static void vb2_to_mmal_buffer(struct mmal_buffer *buf,
 355 +                              struct vb2_v4l2_buffer *vb2)
 356 +{
 357 +       u64 pts;
 358 +
 359 +       buf->mmal_flags = 0;
 360 +       if (vb2->flags & V4L2_BUF_FLAG_KEYFRAME)
 361 +               buf->mmal_flags |= MMAL_BUFFER_HEADER_FLAG_KEYFRAME;
 362 +
 363 +       /* Data must be framed correctly as one frame per buffer. */
 364 +       buf->mmal_flags |= MMAL_BUFFER_HEADER_FLAG_FRAME_END;
 365 +
 366 +       buf->length = vb2->vb2_buf.planes[0].bytesused;
 367 +       /*
 368 +        * Minor ambiguity in the V4L2 spec as to whether passing in a 0 length
 369 +        * buffer, or one with V4L2_BUF_FLAG_LAST set denotes end of stream.
 370 +        * Handle either.
 371 +        */
 372 +       if (!buf->length || vb2->flags & V4L2_BUF_FLAG_LAST)
 373 +               buf->mmal_flags |= MMAL_BUFFER_HEADER_FLAG_EOS;
 374 +
 375 +       /* vb2 timestamps in nsecs, mmal in usecs */
 376 +       pts = vb2->vb2_buf.timestamp;
 377 +       do_div(pts, 1000);
 378 +       buf->pts = pts;
 379 +       buf->dts = MMAL_TIME_UNKNOWN;
 380 +}
 381 +
 382 +static void mmal_buffer_cb(struct vchiq_mmal_instance *instance,
 383 +                          struct vchiq_mmal_port *port, int status,
 384 +                          struct mmal_buffer *mmal_buf)
 385 +{
 386 +       struct bcm2835_isp_buffer *q_buf;
 387 +       struct bcm2835_isp_node *node = port->cb_ctx;
 388 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 389 +       struct vb2_v4l2_buffer *vb2;
 390 +
 391 +       q_buf = container_of(mmal_buf, struct bcm2835_isp_buffer, mmal);
 392 +       vb2 = &q_buf->vb;
 393 +       v4l2_dbg(2, debug, &dev->v4l2_dev,
 394 +                "%s: port:%s[%d], status:%d, buf:%p, dmabuf:%p, length:%lu, flags %u, pts %lld\n",
 395 +                __func__, node_is_output(node) ? "input" : "output", node->id,
 396 +                status, mmal_buf, mmal_buf->dma_buf, mmal_buf->length,
 397 +                mmal_buf->mmal_flags, mmal_buf->pts);
 398 +
 399 +       if (mmal_buf->cmd)
 400 +               v4l2_err(&dev->v4l2_dev,
 401 +                        "%s: Unexpected event on output callback - %08x\n",
 402 +                        __func__, mmal_buf->cmd);
 403 +
 404 +       if (status) {
 405 +               /* error in transfer */
 406 +               if (vb2) {
 407 +                       /* there was a buffer with the error so return it */
 408 +                       vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_ERROR);
 409 +               }
 410 +               return;
 411 +       }
 412 +
 413 +       /* vb2 timestamps in nsecs, mmal in usecs */
 414 +       vb2->vb2_buf.timestamp = mmal_buf->pts * 1000;
 415 +       vb2->sequence = node->sequence++;
 416 +       vb2_set_plane_payload(&vb2->vb2_buf, 0, mmal_buf->length);
 417 +       vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_DONE);
 418 +
 419 +       if (!port->enabled)
 420 +               complete(&dev->frame_cmplt);
 421 +}
 422 +
 423 +static void setup_mmal_port_format(struct bcm2835_isp_node *node,
 424 +                                  struct vchiq_mmal_port *port)
 425 +{
 426 +       struct bcm2835_isp_q_data *q_data = &node->q_data;
 427 +
 428 +       port->format.encoding = q_data->fmt->mmal_fmt;
 429 +       /* Raw image format - set width/height */
 430 +       port->es.video.width = (q_data->bytesperline << 3) / q_data->fmt->depth;
 431 +       port->es.video.height = q_data->height;
 432 +       port->es.video.crop.width = q_data->width;
 433 +       port->es.video.crop.height = q_data->height;
 434 +       port->es.video.crop.x = 0;
 435 +       port->es.video.crop.y = 0;
 436 +};
 437 +
 438 +static int setup_mmal_port(struct bcm2835_isp_node *node)
 439 +{
 440 +       struct vchiq_mmal_port *port = get_port_data(node);
 441 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 442 +       unsigned int enable = 1;
 443 +       int ret;
 444 +
 445 +       v4l2_dbg(2, debug, &dev->v4l2_dev, "%s: setup %s[%d]\n", __func__,
 446 +                node->name, node->id);
 447 +
 448 +       vchiq_mmal_port_parameter_set(dev->mmal_instance, port,
 449 +                                     MMAL_PARAMETER_ZERO_COPY, &enable,
 450 +                                     sizeof(enable));
 451 +       setup_mmal_port_format(node, port);
 452 +       ret = vchiq_mmal_port_set_format(dev->mmal_instance, port);
 453 +       if (ret < 0) {
 454 +               v4l2_dbg(1, debug, &dev->v4l2_dev,
 455 +                        "%s: vchiq_mmal_port_set_format failed\n",
 456 +                        __func__);
 457 +               return ret;
 458 +       }
 459 +
 460 +       if (node->q_data.sizeimage < port->minimum_buffer.size) {
 461 +               v4l2_err(&dev->v4l2_dev,
 462 +                        "buffer size mismatch sizeimage %u < min size %u\n",
 463 +                        node->q_data.sizeimage, port->minimum_buffer.size);
 464 +               return -EINVAL;
 465 +       }
 466 +
 467 +       return 0;
 468 +}
 469 +
 470 +static int bcm2835_isp_mmal_buf_cleanup(struct mmal_buffer *mmal_buf)
 471 +{
 472 +       mmal_vchi_buffer_cleanup(mmal_buf);
 473 +
 474 +       if (mmal_buf->dma_buf) {
 475 +               dma_buf_put(mmal_buf->dma_buf);
 476 +               mmal_buf->dma_buf = NULL;
 477 +       }
 478 +
 479 +       return 0;
 480 +}
 481 +
 482 +static int bcm2835_isp_node_queue_setup(struct vb2_queue *q,
 483 +                                       unsigned int *nbuffers,
 484 +                                       unsigned int *nplanes,
 485 +                                       unsigned int sizes[],
 486 +                                       struct device *alloc_devs[])
 487 +{
 488 +       struct bcm2835_isp_node *node = vb2_get_drv_priv(q);
 489 +       struct vchiq_mmal_port *port;
 490 +       unsigned int size;
 491 +
 492 +       if (setup_mmal_port(node))
 493 +               return -EINVAL;
 494 +
 495 +       size = node->q_data.sizeimage;
 496 +       if (size == 0) {
 497 +               v4l2_info(&node_get_dev(node)->v4l2_dev,
 498 +                         "%s: Image size unset in queue_setup for node %s[%d]\n",
 499 +                         __func__, node->name, node->id);
 500 +               return -EINVAL;
 501 +       }
 502 +
 503 +       if (*nplanes)
 504 +               return sizes[0] < size ? -EINVAL : 0;
 505 +
 506 +       *nplanes = 1;
 507 +       sizes[0] = size;
 508 +
 509 +       port = get_port_data(node);
 510 +       port->current_buffer.size = size;
 511 +
 512 +       if (*nbuffers < port->minimum_buffer.num)
 513 +               *nbuffers = port->minimum_buffer.num;
 514 +
 515 +       port->current_buffer.num = *nbuffers;
 516 +
 517 +       v4l2_dbg(2, debug, &node_get_dev(node)->v4l2_dev,
 518 +                "%s: Image size %u, nbuffers %u for node %s[%d]\n",
 519 +                __func__, sizes[0], *nbuffers, node->name, node->id);
 520 +       return 0;
 521 +}
 522 +
 523 +static int bcm2835_isp_buf_init(struct vb2_buffer *vb)
 524 +{
 525 +       struct bcm2835_isp_node *node = vb2_get_drv_priv(vb->vb2_queue);
 526 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 527 +       struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 528 +       struct bcm2835_isp_buffer *buf =
 529 +               container_of(vb2, struct bcm2835_isp_buffer, vb);
 530 +
 531 +       v4l2_dbg(3, debug, &dev->v4l2_dev, "%s: vb %p\n", __func__, vb);
 532 +
 533 +       buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
 534 +       buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
 535 +       mmal_vchi_buffer_init(dev->mmal_instance, &buf->mmal);
 536 +       return 0;
 537 +}
 538 +
 539 +static int bcm2835_isp_buf_prepare(struct vb2_buffer *vb)
 540 +{
 541 +       struct bcm2835_isp_node *node = vb2_get_drv_priv(vb->vb2_queue);
 542 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 543 +       struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 544 +       struct bcm2835_isp_buffer *buf =
 545 +               container_of(vb2, struct bcm2835_isp_buffer, vb);
 546 +       struct dma_buf *dma_buf;
 547 +       int ret;
 548 +
 549 +       v4l2_dbg(3, debug, &dev->v4l2_dev, "%s: type: %d ptr %p\n",
 550 +                __func__, vb->vb2_queue->type, vb);
 551 +
 552 +       if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
 553 +               if (vb2->field == V4L2_FIELD_ANY)
 554 +                       vb2->field = V4L2_FIELD_NONE;
 555 +               if (vb2->field != V4L2_FIELD_NONE) {
 556 +                       v4l2_err(&dev->v4l2_dev,
 557 +                                "%s field isn't supported\n", __func__);
 558 +                       return -EINVAL;
 559 +               }
 560 +       }
 561 +
 562 +       if (vb2_plane_size(vb, 0) < node->q_data.sizeimage) {
 563 +               v4l2_err(&dev->v4l2_dev,
 564 +                        "%s data will not fit into plane (%lu < %lu)\n",
 565 +                        __func__, vb2_plane_size(vb, 0),
 566 +                        (long)node->q_data.sizeimage);
 567 +               return -EINVAL;
 568 +       }
 569 +
 570 +       if (!V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type))
 571 +               vb2_set_plane_payload(vb, 0, node->q_data.sizeimage);
 572 +
 573 +       switch (vb->memory) {
 574 +       case VB2_MEMORY_DMABUF:
 575 +               dma_buf = dma_buf_get(vb->planes[0].m.fd);
 576 +
 577 +               if (dma_buf != buf->mmal.dma_buf) {
 578 +                       /*
 579 +                        * dmabuf either hasn't already been mapped, or it has
 580 +                        * changed.
 581 +                        */
 582 +                       if (buf->mmal.dma_buf) {
 583 +                               v4l2_err(&dev->v4l2_dev,
 584 +                                        "%s Buffer changed - why did the core not call cleanup?\n",
 585 +                                        __func__);
 586 +                               bcm2835_isp_mmal_buf_cleanup(&buf->mmal);
 587 +                       }
 588 +
 589 +                       buf->mmal.dma_buf = dma_buf;
 590 +               } else {
 591 +                       /*
 592 +                        * Already have a reference to the buffer, so release it
 593 +                        * here.
 594 +                        */
 595 +                       dma_buf_put(dma_buf);
 596 +               }
 597 +               ret = 0;
 598 +               break;
 599 +       case VB2_MEMORY_MMAP:
 600 +               /*
 601 +                * We want to do this at init, but vb2_core_expbuf checks that
 602 +                * the index < q->num_buffers, and q->num_buffers only gets
 603 +                * updated once all the buffers are allocated.
 604 +                */
 605 +               if (!buf->mmal.dma_buf) {
 606 +                       ret = vb2_core_expbuf_dmabuf(vb->vb2_queue,
 607 +                                                    vb->vb2_queue->type,
 608 +                                                    vb->index, 0, O_CLOEXEC,
 609 +                                                    &buf->mmal.dma_buf);
 610 +                       v4l2_dbg(3, debug, &dev->v4l2_dev,
 611 +                                "%s: exporting ptr %p to dmabuf %p\n",
 612 +                                __func__, vb, buf->mmal.dma_buf);
 613 +                       if (ret)
 614 +                               v4l2_err(&dev->v4l2_dev,
 615 +                                        "%s: Failed to expbuf idx %d, ret %d\n",
 616 +                                        __func__, vb->index, ret);
 617 +               } else {
 618 +                       ret = 0;
 619 +               }
 620 +               break;
 621 +       default:
 622 +               ret = -EINVAL;
 623 +               break;
 624 +       }
 625 +
 626 +       return ret;
 627 +}
 628 +
 629 +static void bcm2835_isp_node_buffer_queue(struct vb2_buffer *buf)
 630 +{
 631 +       struct bcm2835_isp_node *node = vb2_get_drv_priv(buf->vb2_queue);
 632 +       struct vb2_v4l2_buffer *vbuf =
 633 +               container_of(buf, struct vb2_v4l2_buffer, vb2_buf);
 634 +       struct bcm2835_isp_buffer *buffer =
 635 +               container_of(vbuf, struct bcm2835_isp_buffer, vb);
 636 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 637 +
 638 +       v4l2_dbg(3, debug, &dev->v4l2_dev, "%s: node %s[%d], buffer %p\n",
 639 +                __func__, node->name, node->id, buffer);
 640 +
 641 +       vb2_to_mmal_buffer(&buffer->mmal, &buffer->vb);
 642 +       v4l2_dbg(3, debug, &dev->v4l2_dev,
 643 +                "%s: node %s[%d] - submitting  mmal dmabuf %p\n", __func__,
 644 +                node->name, node->id, buffer->mmal.dma_buf);
 645 +       vchiq_mmal_submit_buffer(dev->mmal_instance, get_port_data(node),
 646 +                                &buffer->mmal);
 647 +}
 648 +
 649 +static void bcm2835_isp_buffer_cleanup(struct vb2_buffer *vb)
 650 +{
 651 +       struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 652 +       struct bcm2835_isp_buffer *buffer =
 653 +               container_of(vb2, struct bcm2835_isp_buffer, vb);
 654 +
 655 +       bcm2835_isp_mmal_buf_cleanup(&buffer->mmal);
 656 +}
 657 +
 658 +static int bcm2835_isp_node_start_streaming(struct vb2_queue *q,
 659 +                                           unsigned int count)
 660 +{
 661 +       struct bcm2835_isp_node *node = vb2_get_drv_priv(q);
 662 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 663 +       struct vchiq_mmal_port *port = get_port_data(node);
 664 +       int ret;
 665 +
 666 +       v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: node %s[%d] (count %u)\n",
 667 +                __func__, node->name, node->id, count);
 668 +
 669 +       ret = vchiq_mmal_component_enable(dev->mmal_instance, dev->component);
 670 +       if (ret) {
 671 +               v4l2_err(&dev->v4l2_dev, "%s: Failed enabling component, ret %d\n",
 672 +                        __func__, ret);
 673 +               return -EIO;
 674 +       }
 675 +
 676 +       node->sequence = 0;
 677 +       port->cb_ctx = node;
 678 +       ret = vchiq_mmal_port_enable(dev->mmal_instance, port,
 679 +                                    mmal_buffer_cb);
 680 +       if (!ret)
 681 +               atomic_inc(&dev->num_streaming);
 682 +       else
 683 +               v4l2_err(&dev->v4l2_dev,
 684 +                        "%s: Failed enabling port, ret %d\n", __func__, ret);
 685 +
 686 +       return ret;
 687 +}
 688 +
 689 +static void bcm2835_isp_node_stop_streaming(struct vb2_queue *q)
 690 +{
 691 +       struct bcm2835_isp_node *node = vb2_get_drv_priv(q);
 692 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 693 +       struct vchiq_mmal_port *port = get_port_data(node);
 694 +       unsigned int i;
 695 +       int ret;
 696 +
 697 +       v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: node %s[%d], mmal port %p\n",
 698 +                __func__, node->name, node->id, port);
 699 +
 700 +       init_completion(&dev->frame_cmplt);
 701 +
 702 +       /* Disable MMAL port - this will flush buffers back */
 703 +       ret = vchiq_mmal_port_disable(dev->mmal_instance, port);
 704 +       if (ret)
 705 +               v4l2_err(&dev->v4l2_dev,
 706 +                        "%s: Failed disabling %s port, ret %d\n", __func__,
 707 +                        node_is_output(node) ? "i/p" : "o/p",
 708 +                        ret);
 709 +
 710 +       while (atomic_read(&port->buffers_with_vpu)) {
 711 +               v4l2_dbg(1, debug, &dev->v4l2_dev,
 712 +                        "%s: Waiting for buffers to be returned - %d outstanding\n",
 713 +                        __func__, atomic_read(&port->buffers_with_vpu));
 714 +               ret = wait_for_completion_timeout(&dev->frame_cmplt, HZ);
 715 +               if (ret <= 0) {
 716 +                       v4l2_err(&dev->v4l2_dev,
 717 +                                "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
 718 +                                __func__,
 719 +                                atomic_read(&port->buffers_with_vpu));
 720 +                       break;
 721 +               }
 722 +       }
 723 +
 724 +       /* Release the VCSM handle here to release the associated dmabuf */
 725 +       for (i = 0; i < q->num_buffers; i++) {
 726 +               struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(q->bufs[i]);
 727 +               struct bcm2835_isp_buffer *buf =
 728 +                       container_of(vb2, struct bcm2835_isp_buffer, vb);
 729 +               bcm2835_isp_mmal_buf_cleanup(&buf->mmal);
 730 +       }
 731 +
 732 +       atomic_dec(&dev->num_streaming);
 733 +       /* If all ports disabled, then disable the component */
 734 +       if (atomic_read(&dev->num_streaming) == 0) {
 735 +               ret = vchiq_mmal_component_disable(dev->mmal_instance,
 736 +                                                  dev->component);
 737 +               if (ret) {
 738 +                       v4l2_err(&dev->v4l2_dev,
 739 +                                "%s: Failed disabling component, ret %d\n",
 740 +                                __func__, ret);
 741 +               }
 742 +       }
 743 +
 744 +       /*
 745 +        * Simply wait for any vb2 buffers to finish. We could take steps to
 746 +        * make them complete more quickly if we care, or even return them
 747 +        * ourselves.
 748 +        */
 749 +       vb2_wait_for_all_buffers(&node->queue);
 750 +}
 751 +
 752 +static const struct vb2_ops bcm2835_isp_node_queue_ops = {
 753 +       .queue_setup            = bcm2835_isp_node_queue_setup,
 754 +       .buf_init               = bcm2835_isp_buf_init,
 755 +       .buf_prepare            = bcm2835_isp_buf_prepare,
 756 +       .buf_queue              = bcm2835_isp_node_buffer_queue,
 757 +       .buf_cleanup            = bcm2835_isp_buffer_cleanup,
 758 +       .start_streaming        = bcm2835_isp_node_start_streaming,
 759 +       .stop_streaming         = bcm2835_isp_node_stop_streaming,
 760 +};
 761 +
 762 +static struct bcm2835_isp_fmt *get_default_format(struct bcm2835_isp_node *node)
 763 +{
 764 +       return &node->supported_fmts.list[0];
 765 +}
 766 +
 767 +static inline unsigned int get_bytesperline(int width,
 768 +                                           struct bcm2835_isp_fmt *fmt)
 769 +{
 770 +       return ALIGN((width * fmt->depth) >> 3, fmt->bytesperline_align);
 771 +}
 772 +
 773 +static inline unsigned int get_sizeimage(int bpl, int width, int height,
 774 +                                        struct bcm2835_isp_fmt *fmt)
 775 +{
 776 +       return (bpl * height * fmt->size_multiplier_x2) >> 1;
 777 +}
 778 +
 779 +static int bcm2835_isp_s_ctrl(struct v4l2_ctrl *ctrl)
 780 +{
 781 +       struct bcm2835_isp_dev *dev =
 782 +             container_of(ctrl->handler, struct bcm2835_isp_dev, ctrl_handler);
 783 +       struct bcm2835_isp_node *node = &dev->node[0];
 784 +       int ret = 0;
 785 +
 786 +       /*
 787 +        * The ISP firmware driver will ensure these settings are applied on
 788 +        * a frame boundary, so we are safe to write them as they come in.
 789 +        *
 790 +        * Note that the bcm2835_isp_* param structures are identical to the
 791 +        * mmal-parameters.h definitions.  This avoids the need for unnecessary
 792 +        * field-by-field copying between structures.
 793 +        */
 794 +       switch (ctrl->id) {
 795 +       case V4L2_CID_RED_BALANCE:
 796 +               dev->r_gain = ctrl->val;
 797 +               ret = set_wb_gains(node);
 798 +               break;
 799 +       case V4L2_CID_BLUE_BALANCE:
 800 +               dev->b_gain = ctrl->val;
 801 +               ret = set_wb_gains(node);
 802 +               break;
 803 +       case V4L2_CID_DIGITAL_GAIN:
 804 +               ret = set_digital_gain(node, ctrl->val);
 805 +               break;
 806 +       case V4L2_CID_USER_BCM2835_ISP_CC_MATRIX:
 807 +               ret = set_isp_param(node, MMAL_PARAMETER_CUSTOM_CCM,
 808 +                                   ctrl->p_new.p_u8,
 809 +                                   sizeof(struct bcm2835_isp_custom_ccm));
 810 +               break;
 811 +       case V4L2_CID_USER_BCM2835_ISP_LENS_SHADING:
 812 +               ret = set_isp_param(node, MMAL_PARAMETER_LENS_SHADING_OVERRIDE,
 813 +                                   ctrl->p_new.p_u8,
 814 +                                   sizeof(struct bcm2835_isp_lens_shading));
 815 +               break;
 816 +       case V4L2_CID_USER_BCM2835_ISP_BLACK_LEVEL:
 817 +               ret = set_isp_param(node, MMAL_PARAMETER_BLACK_LEVEL,
 818 +                                   ctrl->p_new.p_u8,
 819 +                                   sizeof(struct bcm2835_isp_black_level));
 820 +               break;
 821 +       case V4L2_CID_USER_BCM2835_ISP_GEQ:
 822 +               ret = set_isp_param(node, MMAL_PARAMETER_GEQ,
 823 +                                   ctrl->p_new.p_u8,
 824 +                                   sizeof(struct bcm2835_isp_geq));
 825 +               break;
 826 +       case V4L2_CID_USER_BCM2835_ISP_GAMMA:
 827 +               ret = set_isp_param(node, MMAL_PARAMETER_GAMMA,
 828 +                                   ctrl->p_new.p_u8,
 829 +                                   sizeof(struct bcm2835_isp_gamma));
 830 +               break;
 831 +       case V4L2_CID_USER_BCM2835_ISP_DENOISE:
 832 +               ret = set_isp_param(node, MMAL_PARAMETER_DENOISE,
 833 +                                   ctrl->p_new.p_u8,
 834 +                                   sizeof(struct bcm2835_isp_denoise));
 835 +               break;
 836 +       case V4L2_CID_USER_BCM2835_ISP_SHARPEN:
 837 +               ret = set_isp_param(node, MMAL_PARAMETER_SHARPEN,
 838 +                                   ctrl->p_new.p_u8,
 839 +                                   sizeof(struct bcm2835_isp_sharpen));
 840 +               break;
 841 +       case V4L2_CID_USER_BCM2835_ISP_DPC:
 842 +               ret = set_isp_param(node, MMAL_PARAMETER_DPC,
 843 +                                   ctrl->p_new.p_u8,
 844 +                                   sizeof(struct bcm2835_isp_dpc));
 845 +               break;
 846 +       default:
 847 +               v4l2_info(&dev->v4l2_dev, "Unrecognised control\n");
 848 +               ret = -EINVAL;
 849 +       }
 850 +
 851 +       if (ret) {
 852 +               v4l2_err(&dev->v4l2_dev, "%s: Failed setting ctrl \"%s\" (%08x), err %d\n",
 853 +                        __func__, ctrl->name, ctrl->id, ret);
 854 +               ret = -EIO;
 855 +       }
 856 +
 857 +       return ret;
 858 +}
 859 +
 860 +static const struct v4l2_ctrl_ops bcm2835_isp_ctrl_ops = {
 861 +       .s_ctrl = bcm2835_isp_s_ctrl,
 862 +};
 863 +
 864 +static const struct v4l2_file_operations bcm2835_isp_fops = {
 865 +       .owner          = THIS_MODULE,
 866 +       .open           = v4l2_fh_open,
 867 +       .release        = vb2_fop_release,
 868 +       .poll           = vb2_fop_poll,
 869 +       .unlocked_ioctl = video_ioctl2,
 870 +       .mmap           = vb2_fop_mmap
 871 +};
 872 +
 873 +static int populate_qdata_fmt(struct v4l2_format *f,
 874 +                             struct bcm2835_isp_node *node)
 875 +{
 876 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
 877 +       struct bcm2835_isp_q_data *q_data = &node->q_data;
 878 +       struct vchiq_mmal_port *port;
 879 +       int ret;
 880 +
 881 +       if (!node_is_stats(node)) {
 882 +               v4l2_dbg(1, debug, &dev->v4l2_dev,
 883 +                        "%s: Setting pix format for type %d, wxh: %ux%u, fmt: %08x, size %u\n",
 884 +                        __func__, f->type, f->fmt.pix.width, f->fmt.pix.height,
 885 +                        f->fmt.pix.pixelformat, f->fmt.pix.sizeimage);
 886 +
 887 +               q_data->fmt = find_format(f, node);
 888 +               q_data->width = f->fmt.pix.width;
 889 +               q_data->height = f->fmt.pix.height;
 890 +               q_data->height = f->fmt.pix.height;
 891 +
 892 +               /* All parameters should have been set correctly by try_fmt */
 893 +               q_data->bytesperline = f->fmt.pix.bytesperline;
 894 +               q_data->sizeimage = f->fmt.pix.sizeimage;
 895 +       } else {
 896 +               v4l2_dbg(1, debug, &dev->v4l2_dev,
 897 +                        "%s: Setting meta format for fmt: %08x, size %u\n",
 898 +                        __func__, f->fmt.meta.dataformat,
 899 +                        f->fmt.meta.buffersize);
 900 +
 901 +               q_data->fmt = find_format(f, node);
 902 +               q_data->width = 0;
 903 +               q_data->height = 0;
 904 +               q_data->bytesperline = 0;
 905 +               q_data->sizeimage = f->fmt.meta.buffersize;
 906 +       }
 907 +
 908 +       v4l2_dbg(1, debug, &dev->v4l2_dev,
 909 +                "%s: Calculated bpl as %u, size %u\n", __func__,
 910 +                q_data->bytesperline, q_data->sizeimage);
 911 +
 912 +       /* If we have a component then setup the port as well */
 913 +       port = get_port_data(node);
 914 +       setup_mmal_port_format(node, port);
 915 +       ret = vchiq_mmal_port_set_format(dev->mmal_instance, port);
 916 +       if (ret) {
 917 +               v4l2_err(&dev->v4l2_dev,
 918 +                        "%s: Failed vchiq_mmal_port_set_format on port, ret %d\n",
 919 +                        __func__, ret);
 920 +               ret = -EINVAL;
 921 +       }
 922 +
 923 +       if (q_data->sizeimage < port->minimum_buffer.size) {
 924 +               v4l2_err(&dev->v4l2_dev,
 925 +                        "%s: Current buffer size of %u < min buf size %u - driver mismatch to MMAL\n",
 926 +                        __func__,
 927 +                        q_data->sizeimage,
 928 +                        port->minimum_buffer.size);
 929 +       }
 930 +
 931 +       v4l2_dbg(1, debug, &dev->v4l2_dev,
 932 +                "%s: Set format for type %d, wxh: %dx%d, fmt: %08x, size %u\n",
 933 +                __func__, f->type, q_data->width, q_data->height,
 934 +                q_data->fmt->fourcc, q_data->sizeimage);
 935 +
 936 +       return ret;
 937 +}
 938 +
 939 +static int bcm2835_isp_node_querycap(struct file *file, void *priv,
 940 +                                    struct v4l2_capability *cap)
 941 +{
 942 +       strscpy(cap->driver, BCM2835_ISP_NAME, sizeof(cap->driver));
 943 +       strscpy(cap->card, BCM2835_ISP_NAME, sizeof(cap->card));
 944 +       snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
 945 +                BCM2835_ISP_NAME);
 946 +
 947 +       return 0;
 948 +}
 949 +
 950 +static int bcm2835_isp_node_g_fmt(struct file *file, void *priv,
 951 +                                 struct v4l2_format *f)
 952 +{
 953 +       struct bcm2835_isp_node *node = video_drvdata(file);
 954 +
 955 +       if (f->type != node->queue.type)
 956 +               return -EINVAL;
 957 +
 958 +       if (node_is_stats(node)) {
 959 +               f->fmt.meta.dataformat = V4L2_META_FMT_BCM2835_ISP_STATS;
 960 +               f->fmt.meta.buffersize =
 961 +                       get_port_data(node)->minimum_buffer.size;
 962 +       } else {
 963 +               struct bcm2835_isp_q_data *q_data = &node->q_data;
 964 +
 965 +               f->fmt.pix.width = q_data->width;
 966 +               f->fmt.pix.height = q_data->height;
 967 +               f->fmt.pix.field = V4L2_FIELD_NONE;
 968 +               f->fmt.pix.pixelformat = q_data->fmt->fourcc;
 969 +               f->fmt.pix.bytesperline = q_data->bytesperline;
 970 +               f->fmt.pix.sizeimage = q_data->sizeimage;
 971 +               f->fmt.pix.colorspace = q_data->fmt->colorspace;
 972 +       }
 973 +
 974 +       return 0;
 975 +}
 976 +
 977 +static int bcm2835_isp_node_enum_fmt(struct file *file, void  *priv,
 978 +                                    struct v4l2_fmtdesc *f)
 979 +{
 980 +       struct bcm2835_isp_node *node = video_drvdata(file);
 981 +       struct bcm2835_isp_fmt_list *fmts = &node->supported_fmts;
 982 +
 983 +       if (f->type != node->queue.type)
 984 +               return -EINVAL;
 985 +
 986 +       if (f->index < fmts->num_entries) {
 987 +               /* Format found */
 988 +               f->pixelformat = fmts->list[f->index].fourcc;
 989 +               f->flags = fmts->list[f->index].flags;
 990 +               return 0;
 991 +       }
 992 +
 993 +       return -EINVAL;
 994 +}
 995 +
 996 +static int bcm2835_isp_node_try_fmt(struct file *file, void *priv,
 997 +                                   struct v4l2_format *f)
 998 +{
 999 +       struct bcm2835_isp_node *node = video_drvdata(file);
1000 +       struct bcm2835_isp_fmt *fmt;
1001 +
1002 +       if (f->type != node->queue.type)
1003 +               return -EINVAL;
1004 +
1005 +       fmt = find_format(f, node);
1006 +       if (!fmt)
1007 +               fmt = get_default_format(node);
1008 +
1009 +       if (!node_is_stats(node)) {
1010 +               f->fmt.pix.width = max(min(f->fmt.pix.width, MAX_DIM),
1011 +                                      MIN_DIM);
1012 +               f->fmt.pix.height = max(min(f->fmt.pix.height, MAX_DIM),
1013 +                                       MIN_DIM);
1014 +
1015 +               f->fmt.pix.pixelformat = fmt->fourcc;
1016 +               f->fmt.pix.colorspace = fmt->colorspace;
1017 +               f->fmt.pix.bytesperline = get_bytesperline(f->fmt.pix.width,
1018 +                                                          fmt);
1019 +               f->fmt.pix.field = V4L2_FIELD_NONE;
1020 +               f->fmt.pix.sizeimage =
1021 +                       get_sizeimage(f->fmt.pix.bytesperline, f->fmt.pix.width,
1022 +                                     f->fmt.pix.height, fmt);
1023 +       } else {
1024 +               f->fmt.meta.dataformat = fmt->fourcc;
1025 +               f->fmt.meta.buffersize =
1026 +                               get_port_data(node)->minimum_buffer.size;
1027 +       }
1028 +
1029 +       return 0;
1030 +}
1031 +
1032 +static int bcm2835_isp_node_s_fmt(struct file *file, void *priv,
1033 +                                 struct v4l2_format *f)
1034 +{
1035 +       struct bcm2835_isp_node *node = video_drvdata(file);
1036 +       int ret;
1037 +
1038 +       if (f->type != node->queue.type)
1039 +               return -EINVAL;
1040 +
1041 +       ret = bcm2835_isp_node_try_fmt(file, priv, f);
1042 +       if (ret)
1043 +               return ret;
1044 +
1045 +       v4l2_dbg(1, debug, &node_get_dev(node)->v4l2_dev,
1046 +                "%s: Set format for node %s[%d]\n",
1047 +                __func__, node->name, node->id);
1048 +
1049 +       return populate_qdata_fmt(f, node);
1050 +}
1051 +
1052 +static int bcm2835_isp_node_s_selection(struct file *file, void *fh,
1053 +                                       struct v4l2_selection *s)
1054 +{
1055 +       struct mmal_parameter_crop crop;
1056 +       struct bcm2835_isp_node *node = video_drvdata(file);
1057 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
1058 +       struct vchiq_mmal_port *port = get_port_data(node);
1059 +
1060 +       /* This return value is required fro V4L2 compliance. */
1061 +       if (node_is_stats(node))
1062 +               return -ENOTTY;
1063 +
1064 +       if (!s->r.width || !s->r.height)
1065 +               return -EINVAL;
1066 +
1067 +       /* Adjust the crop window if goes outside the frame dimensions. */
1068 +       s->r.left = min((unsigned int)max(s->r.left, 0),
1069 +                       node->q_data.width - MIN_DIM);
1070 +       s->r.top = min((unsigned int)max(s->r.top, 0),
1071 +                      node->q_data.height - MIN_DIM);
1072 +       s->r.width = max(min(s->r.width, node->q_data.width - s->r.left),
1073 +                        MIN_DIM);
1074 +       s->r.height = max(min(s->r.height, node->q_data.height - s->r.top),
1075 +                         MIN_DIM);
1076 +
1077 +       crop.rect.x = s->r.left;
1078 +       crop.rect.y = s->r.top;
1079 +       crop.rect.width = s->r.width;
1080 +       crop.rect.height = s->r.height;
1081 +
1082 +       return vchiq_mmal_port_parameter_set(dev->mmal_instance, port,
1083 +                                            MMAL_PARAMETER_CROP,
1084 +                                            &crop, sizeof(crop));
1085 +}
1086 +
1087 +static int bcm2835_isp_node_g_selection(struct file *file, void *fh,
1088 +                                       struct v4l2_selection *s)
1089 +{
1090 +       struct bcm2835_isp_node *node = video_drvdata(file);
1091 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
1092 +       struct vchiq_mmal_port *port = get_port_data(node);
1093 +       struct mmal_parameter_crop crop;
1094 +       u32 crop_size = sizeof(crop);
1095 +       int ret;
1096 +
1097 +       /* This return value is required for V4L2 compliance. */
1098 +       if (node_is_stats(node))
1099 +               return -ENOTTY;
1100 +
1101 +       /* We can only return out an input crop. */
1102 +       if (s->target != V4L2_SEL_TGT_CROP)
1103 +               return -EINVAL;
1104 +
1105 +       ret = vchiq_mmal_port_parameter_get(dev->mmal_instance, port,
1106 +                                           MMAL_PARAMETER_CROP,
1107 +                                           &crop, &crop_size);
1108 +       if (!ret)
1109 +               return -EINVAL;
1110 +
1111 +       s->r.left = crop.rect.x;
1112 +       s->r.top = crop.rect.y;
1113 +       s->r.width = crop.rect.width;
1114 +       s->r.height = crop.rect.height;
1115 +
1116 +       return 0;
1117 +}
1118 +
1119 +static int bcm3285_isp_subscribe_event(struct v4l2_fh *fh,
1120 +                                      const struct v4l2_event_subscription *s)
1121 +{
1122 +       switch (s->type) {
1123 +       /* Cannot change source parameters dynamically at runtime. */
1124 +       case V4L2_EVENT_SOURCE_CHANGE:
1125 +               return -EINVAL;
1126 +       case V4L2_EVENT_CTRL:
1127 +               return v4l2_ctrl_subscribe_event(fh, s);
1128 +       default:
1129 +               return v4l2_event_subscribe(fh, s, 4, NULL);
1130 +       }
1131 +}
1132 +
1133 +static const struct v4l2_ioctl_ops bcm2835_isp_node_ioctl_ops = {
1134 +       .vidioc_querycap                = bcm2835_isp_node_querycap,
1135 +       .vidioc_g_fmt_vid_cap           = bcm2835_isp_node_g_fmt,
1136 +       .vidioc_g_fmt_vid_out           = bcm2835_isp_node_g_fmt,
1137 +       .vidioc_g_fmt_meta_cap          = bcm2835_isp_node_g_fmt,
1138 +       .vidioc_s_fmt_vid_cap           = bcm2835_isp_node_s_fmt,
1139 +       .vidioc_s_fmt_vid_out           = bcm2835_isp_node_s_fmt,
1140 +       .vidioc_s_fmt_meta_cap          = bcm2835_isp_node_s_fmt,
1141 +       .vidioc_try_fmt_vid_cap         = bcm2835_isp_node_try_fmt,
1142 +       .vidioc_try_fmt_vid_out         = bcm2835_isp_node_try_fmt,
1143 +       .vidioc_try_fmt_meta_cap        = bcm2835_isp_node_try_fmt,
1144 +       .vidioc_s_selection             = bcm2835_isp_node_s_selection,
1145 +       .vidioc_g_selection             = bcm2835_isp_node_g_selection,
1146 +
1147 +       .vidioc_enum_fmt_vid_cap        = bcm2835_isp_node_enum_fmt,
1148 +       .vidioc_enum_fmt_vid_out        = bcm2835_isp_node_enum_fmt,
1149 +       .vidioc_enum_fmt_meta_cap       = bcm2835_isp_node_enum_fmt,
1150 +
1151 +       .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1152 +       .vidioc_querybuf                = vb2_ioctl_querybuf,
1153 +       .vidioc_qbuf                    = vb2_ioctl_qbuf,
1154 +       .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1155 +       .vidioc_expbuf                  = vb2_ioctl_expbuf,
1156 +       .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1157 +       .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1158 +
1159 +       .vidioc_streamon                = vb2_ioctl_streamon,
1160 +       .vidioc_streamoff               = vb2_ioctl_streamoff,
1161 +
1162 +       .vidioc_subscribe_event         = bcm3285_isp_subscribe_event,
1163 +       .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1164 +};
1165 +
1166 +/*
1167 + * Size of the array to provide to the VPU when asking for the list of supported
1168 + * formats.
1169 + *
1170 + * The ISP component currently advertises 33 input formats, so add a small
1171 + * overhead on that.
1172 + */
1173 +#define MAX_SUPPORTED_ENCODINGS 40
1174 +
1175 +/* Populate node->supported_fmts with the formats supported by those ports. */
1176 +static int bcm2835_isp_get_supported_fmts(struct bcm2835_isp_node *node)
1177 +{
1178 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
1179 +       struct bcm2835_isp_fmt *list;
1180 +       unsigned int i, j, num_encodings;
1181 +       u32 fourccs[MAX_SUPPORTED_ENCODINGS];
1182 +       u32 param_size = sizeof(fourccs);
1183 +       int ret;
1184 +
1185 +       ret = vchiq_mmal_port_parameter_get(dev->mmal_instance,
1186 +                                           get_port_data(node),
1187 +                                           MMAL_PARAMETER_SUPPORTED_ENCODINGS,
1188 +                                           &fourccs, &param_size);
1189 +
1190 +       if (ret) {
1191 +               if (ret == MMAL_MSG_STATUS_ENOSPC) {
1192 +                       v4l2_err(&dev->v4l2_dev,
1193 +                                "%s: port has more encoding than we provided space for. Some are dropped.\n",
1194 +                                __func__);
1195 +                       num_encodings = MAX_SUPPORTED_ENCODINGS;
1196 +               } else {
1197 +                       v4l2_err(&dev->v4l2_dev, "%s: get_param ret %u.\n",
1198 +                                __func__, ret);
1199 +                       return -EINVAL;
1200 +               }
1201 +       } else {
1202 +               num_encodings = param_size / sizeof(u32);
1203 +       }
1204 +
1205 +       /*
1206 +        * Assume at this stage that all encodings will be supported in V4L2.
1207 +        * Any that aren't supported will waste a very small amount of memory.
1208 +        */
1209 +       list = devm_kzalloc(dev->dev,
1210 +                           sizeof(struct bcm2835_isp_fmt) * num_encodings,
1211 +                           GFP_KERNEL);
1212 +       if (!list)
1213 +               return -ENOMEM;
1214 +       node->supported_fmts.list = list;
1215 +
1216 +       for (i = 0, j = 0; i < num_encodings; i++) {
1217 +               const struct bcm2835_isp_fmt *fmt = get_fmt(fourccs[i]);
1218 +
1219 +               if (fmt) {
1220 +                       list[j] = *fmt;
1221 +                       j++;
1222 +               }
1223 +       }
1224 +       node->supported_fmts.num_entries = j;
1225 +
1226 +       param_size = sizeof(fourccs);
1227 +       ret = vchiq_mmal_port_parameter_get(dev->mmal_instance,
1228 +                                           get_port_data(node),
1229 +                                           MMAL_PARAMETER_SUPPORTED_ENCODINGS,
1230 +                                           &fourccs, &param_size);
1231 +
1232 +       if (ret) {
1233 +               if (ret == MMAL_MSG_STATUS_ENOSPC) {
1234 +                       v4l2_err(&dev->v4l2_dev,
1235 +                                "%s: port has more encoding than we provided space for. Some are dropped.\n",
1236 +                                __func__);
1237 +                       num_encodings = MAX_SUPPORTED_ENCODINGS;
1238 +               } else {
1239 +                       return -EINVAL;
1240 +               }
1241 +       } else {
1242 +               num_encodings = param_size / sizeof(u32);
1243 +       }
1244 +       /* Assume at this stage that all encodings will be supported in V4L2. */
1245 +       list = devm_kzalloc(dev->dev,
1246 +                           sizeof(struct bcm2835_isp_fmt) * num_encodings,
1247 +                           GFP_KERNEL);
1248 +       if (!list)
1249 +               return -ENOMEM;
1250 +       node->supported_fmts.list = list;
1251 +
1252 +       for (i = 0, j = 0; i < num_encodings; i++) {
1253 +               const struct bcm2835_isp_fmt *fmt = get_fmt(fourccs[i]);
1254 +
1255 +               if (fmt) {
1256 +                       list[j] = *fmt;
1257 +                       j++;
1258 +               }
1259 +       }
1260 +       node->supported_fmts.num_entries = j;
1261 +       return 0;
1262 +}
1263 +
1264 +/*
1265 + * Register a device node /dev/video<N> to go along with one of the ISP's input
1266 + * or output nodes.
1267 + */
1268 +static int register_node(struct bcm2835_isp_dev *dev,
1269 +                        struct bcm2835_isp_node *node,
1270 +                        int index)
1271 +{
1272 +       struct video_device *vfd;
1273 +       struct vb2_queue *queue;
1274 +       int ret;
1275 +
1276 +       mutex_init(&node->lock);
1277 +
1278 +       node->dev = dev;
1279 +       vfd = &node->vfd;
1280 +       queue = &node->queue;
1281 +       queue->type = index_to_queue_type(index);
1282 +       /*
1283 +        * Setup the node type-specific params.
1284 +        *
1285 +        * Only the OUTPUT node can set controls and crop windows. However,
1286 +        * we must allow the s/g_selection ioctl on the stats node as v4l2
1287 +        * compliance expects it to return a -ENOTTY, and the framework
1288 +        * does not handle it if the ioctl is disabled.
1289 +        */
1290 +       switch (queue->type) {
1291 +       case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1292 +               vfd->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
1293 +               node->id = index;
1294 +               node->vfl_dir = VFL_DIR_TX;
1295 +               node->name = "output";
1296 +               break;
1297 +       case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1298 +               vfd->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1299 +               /* First Capture node starts at id 0, etc. */
1300 +               node->id = index - BCM2835_ISP_NUM_OUTPUTS;
1301 +               node->vfl_dir = VFL_DIR_RX;
1302 +               node->name = "capture";
1303 +               v4l2_disable_ioctl(&node->vfd, VIDIOC_S_CTRL);
1304 +               v4l2_disable_ioctl(&node->vfd, VIDIOC_S_SELECTION);
1305 +               v4l2_disable_ioctl(&node->vfd, VIDIOC_G_SELECTION);
1306 +               break;
1307 +       case V4L2_BUF_TYPE_META_CAPTURE:
1308 +               vfd->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING;
1309 +               node->id = index - BCM2835_ISP_NUM_OUTPUTS;
1310 +               node->vfl_dir = VFL_DIR_RX;
1311 +               node->name = "stats";
1312 +               v4l2_disable_ioctl(&node->vfd, VIDIOC_S_CTRL);
1313 +               break;
1314 +       }
1315 +
1316 +       /* We use the selection API instead of the old crop API. */
1317 +       v4l2_disable_ioctl(vfd, VIDIOC_CROPCAP);
1318 +       v4l2_disable_ioctl(vfd, VIDIOC_G_CROP);
1319 +       v4l2_disable_ioctl(vfd, VIDIOC_S_CROP);
1320 +
1321 +       ret = bcm2835_isp_get_supported_fmts(node);
1322 +       if (ret)
1323 +               return ret;
1324 +
1325 +       /* Initialise the the video node. */
1326 +       vfd->vfl_type   = VFL_TYPE_GRABBER;
1327 +       vfd->fops       = &bcm2835_isp_fops,
1328 +       vfd->ioctl_ops  = &bcm2835_isp_node_ioctl_ops,
1329 +       vfd->minor      = -1,
1330 +       vfd->release    = video_device_release_empty,
1331 +       vfd->queue      = &node->queue;
1332 +       vfd->lock       = &node->lock;
1333 +       vfd->v4l2_dev   = &dev->v4l2_dev;
1334 +       vfd->vfl_dir    = node->vfl_dir;
1335 +
1336 +       node->q_data.fmt = get_default_format(node);
1337 +       node->q_data.width = DEFAULT_DIM;
1338 +       node->q_data.height = DEFAULT_DIM;
1339 +       node->q_data.bytesperline =
1340 +               get_bytesperline(DEFAULT_DIM, node->q_data.fmt);
1341 +       node->q_data.sizeimage = node_is_stats(node) ?
1342 +                                get_port_data(node)->recommended_buffer.size :
1343 +                                get_sizeimage(node->q_data.bytesperline,
1344 +                                              node->q_data.width,
1345 +                                              node->q_data.height,
1346 +                                              node->q_data.fmt);
1347 +
1348 +       queue->io_modes = VB2_MMAP | VB2_DMABUF;
1349 +       queue->drv_priv = node;
1350 +       queue->ops = &bcm2835_isp_node_queue_ops;
1351 +       queue->mem_ops = &vb2_dma_contig_memops;
1352 +       queue->buf_struct_size = sizeof(struct bcm2835_isp_buffer);
1353 +       queue->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1354 +       queue->dev = dev->dev;
1355 +       queue->lock = &node->queue_lock;
1356 +
1357 +       ret = vb2_queue_init(queue);
1358 +       if (ret < 0) {
1359 +               v4l2_info(&dev->v4l2_dev, "vb2_queue_init failed\n");
1360 +               return ret;
1361 +       }
1362 +       node->queue_init = true;
1363 +
1364 +       /* Define the device names */
1365 +       snprintf(vfd->name, sizeof(node->vfd.name), "%s-%s%d", BCM2835_ISP_NAME,
1366 +                node->name, node->id);
1367 +
1368 +       ret = video_register_device(vfd, VFL_TYPE_GRABBER, video_nr + index);
1369 +       if (ret) {
1370 +               v4l2_err(&dev->v4l2_dev,
1371 +                        "Failed to register video %s[%d] device node\n",
1372 +                        node->name, node->id);
1373 +               return ret;
1374 +       }
1375 +
1376 +       node->registered = true;
1377 +       video_set_drvdata(vfd, node);
1378 +
1379 +       /* Set some controls and defaults, but only on the VIDEO_OUTPUT node. */
1380 +       if (node_is_output(node)) {
1381 +               unsigned int i;
1382 +
1383 +               /* Use this ctrl template to assign all out ISP custom ctrls. */
1384 +               struct v4l2_ctrl_config ctrl_template = {
1385 +                       .ops            = &bcm2835_isp_ctrl_ops,
1386 +                       .type           = V4L2_CTRL_TYPE_U8,
1387 +                       .def            = 0,
1388 +                       .min            = 0x00,
1389 +                       .max            = 0xff,
1390 +                       .step           = 1,
1391 +               };
1392 +
1393 +               v4l2_ctrl_handler_init(&dev->ctrl_handler, 4);
1394 +
1395 +               dev->r_gain = 1000;
1396 +               dev->b_gain = 1000;
1397 +
1398 +               v4l2_ctrl_new_std(&dev->ctrl_handler,  &bcm2835_isp_ctrl_ops,
1399 +                                 V4L2_CID_RED_BALANCE, 1, 0xffff, 1,
1400 +                                 dev->r_gain);
1401 +
1402 +               v4l2_ctrl_new_std(&dev->ctrl_handler, &bcm2835_isp_ctrl_ops,
1403 +                                 V4L2_CID_BLUE_BALANCE, 1, 0xffff, 1,
1404 +                                 dev->b_gain);
1405 +
1406 +               v4l2_ctrl_new_std(&dev->ctrl_handler, &bcm2835_isp_ctrl_ops,
1407 +                                 V4L2_CID_DIGITAL_GAIN, 1, 0xffff, 1, 1000);
1408 +
1409 +               for (i = 0; i < ARRAY_SIZE(custom_ctrls); i++) {
1410 +                       ctrl_template.name = custom_ctrls[i].name;
1411 +                       ctrl_template.id = custom_ctrls[i].id;
1412 +                       ctrl_template.dims[0] = custom_ctrls[i].size;
1413 +                       ctrl_template.flags = custom_ctrls[i].flags;
1414 +                       v4l2_ctrl_new_custom(&dev->ctrl_handler,
1415 +                                            &ctrl_template, NULL);
1416 +               }
1417 +
1418 +               node->vfd.ctrl_handler = &dev->ctrl_handler;
1419 +       }
1420 +
1421 +       v4l2_info(&dev->v4l2_dev,
1422 +                 "Device node %s[%d] registered as /dev/video%d\n",
1423 +                 node->name, node->id, vfd->num);
1424 +
1425 +       return 0;
1426 +}
1427 +
1428 +/* Unregister one of the /dev/video<N> nodes associated with the ISP. */
1429 +static void unregister_node(struct bcm2835_isp_node *node)
1430 +{
1431 +       struct bcm2835_isp_dev *dev = node_get_dev(node);
1432 +
1433 +       v4l2_info(&dev->v4l2_dev,
1434 +                 "Unregistering node %s[%d] device node /dev/video%d\n",
1435 +                 node->name, node->id, node->vfd.num);
1436 +
1437 +       if (node->queue_init)
1438 +               vb2_queue_release(&node->queue);
1439 +
1440 +       if (node->registered) {
1441 +               video_unregister_device(&node->vfd);
1442 +               if (node_is_output(node))
1443 +                       v4l2_ctrl_handler_free(&dev->ctrl_handler);
1444 +       }
1445 +
1446 +       /*
1447 +        * node->supported_fmts.list is free'd automatically
1448 +        * as a managed resource.
1449 +        */
1450 +       node->supported_fmts.list = NULL;
1451 +       node->supported_fmts.num_entries = 0;
1452 +       node->vfd.ctrl_handler = NULL;
1453 +       node->registered = false;
1454 +       node->queue_init = false;
1455 +}
1456 +
1457 +static void media_controller_unregister(struct bcm2835_isp_dev *dev)
1458 +{
1459 +       unsigned int i;
1460 +
1461 +       v4l2_info(&dev->v4l2_dev, "Unregister from media controller\n");
1462 +
1463 +       if (dev->media_device_registered) {
1464 +               media_device_unregister(&dev->mdev);
1465 +               media_device_cleanup(&dev->mdev);
1466 +               dev->media_device_registered = false;
1467 +       }
1468 +
1469 +       kfree(dev->entity.name);
1470 +       dev->entity.name = NULL;
1471 +
1472 +       if (dev->media_entity_registered) {
1473 +               media_device_unregister_entity(&dev->entity);
1474 +               dev->media_entity_registered = false;
1475 +       }
1476 +
1477 +       for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1478 +               struct bcm2835_isp_node *node = &dev->node[i];
1479 +
1480 +               if (node->media_node_registered) {
1481 +                       media_remove_intf_links(node->intf_link->intf);
1482 +                       media_entity_remove_links(&dev->node[i].vfd.entity);
1483 +                       media_devnode_remove(node->intf_devnode);
1484 +                       media_device_unregister_entity(&node->vfd.entity);
1485 +                       kfree(node->vfd.entity.name);
1486 +               }
1487 +               node->media_node_registered = false;
1488 +       }
1489 +
1490 +       dev->v4l2_dev.mdev = NULL;
1491 +}
1492 +
1493 +static int media_controller_register_node(struct bcm2835_isp_dev *dev, int num)
1494 +{
1495 +       struct bcm2835_isp_node *node = &dev->node[num];
1496 +       struct media_entity *entity = &node->vfd.entity;
1497 +       int output = node_is_output(node);
1498 +       char *name;
1499 +       int ret;
1500 +
1501 +       v4l2_info(&dev->v4l2_dev,
1502 +                 "Register %s node %d with media controller\n",
1503 +                 output ? "output" : "capture", num);
1504 +       entity->obj_type = MEDIA_ENTITY_TYPE_VIDEO_DEVICE;
1505 +       entity->function = MEDIA_ENT_F_IO_V4L;
1506 +       entity->info.dev.major = VIDEO_MAJOR;
1507 +       entity->info.dev.minor = node->vfd.minor;
1508 +       name = kmalloc(BCM2835_ISP_ENTITY_NAME_LEN, GFP_KERNEL);
1509 +       if (!name) {
1510 +               ret = -ENOMEM;
1511 +               goto error_no_mem;
1512 +       }
1513 +       snprintf(name, BCM2835_ISP_ENTITY_NAME_LEN, "%s0-%s%d",
1514 +                BCM2835_ISP_NAME, output ? "output" : "capture", num);
1515 +       entity->name = name;
1516 +       node->pad.flags = output ? MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
1517 +       ret = media_entity_pads_init(entity, 1, &node->pad);
1518 +       if (ret)
1519 +               goto error_pads_init;
1520 +       ret = media_device_register_entity(&dev->mdev, entity);
1521 +       if (ret)
1522 +               goto error_register_entity;
1523 +
1524 +       node->intf_devnode = media_devnode_create(&dev->mdev,
1525 +                                                 MEDIA_INTF_T_V4L_VIDEO, 0,
1526 +                                                 VIDEO_MAJOR, node->vfd.minor);
1527 +       if (!node->intf_devnode) {
1528 +               ret = -ENOMEM;
1529 +               goto error_devnode_create;
1530 +       }
1531 +
1532 +       node->intf_link = media_create_intf_link(entity,
1533 +                                                &node->intf_devnode->intf,
1534 +                                                MEDIA_LNK_FL_IMMUTABLE |
1535 +                                                MEDIA_LNK_FL_ENABLED);
1536 +       if (!node->intf_link) {
1537 +               ret = -ENOMEM;
1538 +               goto error_create_intf_link;
1539 +       }
1540 +
1541 +       if (output)
1542 +               ret = media_create_pad_link(entity, 0, &dev->entity, num,
1543 +                                           MEDIA_LNK_FL_IMMUTABLE |
1544 +                                                   MEDIA_LNK_FL_ENABLED);
1545 +       else
1546 +               ret = media_create_pad_link(&dev->entity, num, entity, 0,
1547 +                                           MEDIA_LNK_FL_IMMUTABLE |
1548 +                                           MEDIA_LNK_FL_ENABLED);
1549 +       if (ret)
1550 +               goto error_create_pad_link;
1551 +
1552 +       dev->node[num].media_node_registered = true;
1553 +       return 0;
1554 +
1555 +error_create_pad_link:
1556 +       media_remove_intf_links(&node->intf_devnode->intf);
1557 +error_create_intf_link:
1558 +       media_devnode_remove(node->intf_devnode);
1559 +error_devnode_create:
1560 +       media_device_unregister_entity(&node->vfd.entity);
1561 +error_register_entity:
1562 +error_pads_init:
1563 +       kfree(entity->name);
1564 +       entity->name = NULL;
1565 +error_no_mem:
1566 +       if (ret)
1567 +               v4l2_info(&dev->v4l2_dev, "Error registering node\n");
1568 +
1569 +       return ret;
1570 +}
1571 +
1572 +static int media_controller_register(struct bcm2835_isp_dev *dev)
1573 +{
1574 +       char *name;
1575 +       unsigned int i;
1576 +       int ret;
1577 +
1578 +       v4l2_dbg(2, debug, &dev->v4l2_dev, "Registering with media controller\n");
1579 +       dev->mdev.dev = dev->dev;
1580 +       strscpy(dev->mdev.model, "bcm2835-isp",
1581 +               sizeof(dev->mdev.model));
1582 +       strscpy(dev->mdev.bus_info, "platform:bcm2835-isp",
1583 +               sizeof(dev->mdev.bus_info));
1584 +       media_device_init(&dev->mdev);
1585 +       dev->v4l2_dev.mdev = &dev->mdev;
1586 +
1587 +       v4l2_dbg(2, debug, &dev->v4l2_dev, "Register entity for nodes\n");
1588 +
1589 +       name = kmalloc(BCM2835_ISP_ENTITY_NAME_LEN, GFP_KERNEL);
1590 +       if (!name) {
1591 +               ret = -ENOMEM;
1592 +               goto done;
1593 +       }
1594 +       snprintf(name, BCM2835_ISP_ENTITY_NAME_LEN, "bcm2835_isp0");
1595 +       dev->entity.name = name;
1596 +       dev->entity.obj_type = MEDIA_ENTITY_TYPE_BASE;
1597 +       dev->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
1598 +
1599 +       for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1600 +               dev->pad[i].flags = node_is_output(&dev->node[i]) ?
1601 +                                       MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
1602 +       }
1603 +
1604 +       ret = media_entity_pads_init(&dev->entity, BCM2835_ISP_NUM_NODES,
1605 +                                    dev->pad);
1606 +       if (ret)
1607 +               goto done;
1608 +
1609 +       ret = media_device_register_entity(&dev->mdev, &dev->entity);
1610 +       if (ret)
1611 +               goto done;
1612 +
1613 +       dev->media_entity_registered = true;
1614 +       for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1615 +               ret = media_controller_register_node(dev, i);
1616 +               if (ret)
1617 +                       goto done;
1618 +       }
1619 +
1620 +       ret = media_device_register(&dev->mdev);
1621 +       if (!ret)
1622 +               dev->media_device_registered = true;
1623 +done:
1624 +       return ret;
1625 +}
1626 +
1627 +static int bcm2835_isp_remove(struct platform_device *pdev)
1628 +{
1629 +       struct bcm2835_isp_dev *dev = platform_get_drvdata(pdev);
1630 +       unsigned int i;
1631 +
1632 +       media_controller_unregister(dev);
1633 +
1634 +       for (i = 0; i < BCM2835_ISP_NUM_NODES; i++)
1635 +               unregister_node(&dev->node[i]);
1636 +
1637 +       v4l2_device_unregister(&dev->v4l2_dev);
1638 +
1639 +       if (dev->component)
1640 +               vchiq_mmal_component_finalise(dev->mmal_instance,
1641 +                                             dev->component);
1642 +
1643 +       vchiq_mmal_finalise(dev->mmal_instance);
1644 +
1645 +       return 0;
1646 +}
1647 +
1648 +static int bcm2835_isp_probe(struct platform_device *pdev)
1649 +{
1650 +       struct bcm2835_isp_dev *dev;
1651 +       unsigned int i;
1652 +       int ret;
1653 +
1654 +       dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
1655 +       if (!dev)
1656 +               return -ENOMEM;
1657 +
1658 +       dev->dev = &pdev->dev;
1659 +
1660 +       ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1661 +       if (ret)
1662 +               return ret;
1663 +
1664 +       ret = vchiq_mmal_init(&dev->mmal_instance);
1665 +       if (ret) {
1666 +               v4l2_device_unregister(&dev->v4l2_dev);
1667 +               return ret;
1668 +       }
1669 +
1670 +       ret = vchiq_mmal_component_init(dev->mmal_instance, "ril.isp",
1671 +                                       &dev->component);
1672 +       if (ret) {
1673 +               v4l2_err(&dev->v4l2_dev,
1674 +                        "%s: failed to create ril.isp component\n", __func__);
1675 +               goto error;
1676 +       }
1677 +
1678 +       if ((dev->component->inputs != BCM2835_ISP_NUM_OUTPUTS) ||
1679 +           (dev->component->outputs != BCM2835_ISP_NUM_CAPTURES +
1680 +                                       BCM2835_ISP_NUM_METADATA)) {
1681 +               v4l2_err(&dev->v4l2_dev,
1682 +                        "%s: ril.isp returned %d i/p (%d expected), %d o/p (%d expected) ports\n",
1683 +                         __func__, dev->component->inputs,
1684 +                         BCM2835_ISP_NUM_OUTPUTS,
1685 +                         dev->component->outputs,
1686 +                         BCM2835_ISP_NUM_CAPTURES + BCM2835_ISP_NUM_METADATA);
1687 +               goto error;
1688 +       }
1689 +
1690 +       atomic_set(&dev->num_streaming, 0);
1691 +
1692 +       for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1693 +               struct bcm2835_isp_node *node = &dev->node[i];
1694 +
1695 +               ret = register_node(dev, node, i);
1696 +               if (ret)
1697 +                       goto error;
1698 +       }
1699 +
1700 +       ret = media_controller_register(dev);
1701 +       if (ret)
1702 +               goto error;
1703 +
1704 +       platform_set_drvdata(pdev, dev);
1705 +       v4l2_info(&dev->v4l2_dev, "Loaded V4L2 %s\n", BCM2835_ISP_NAME);
1706 +       return 0;
1707 +
1708 +error:
1709 +       bcm2835_isp_remove(pdev);
1710 +
1711 +       return ret;
1712 +}
1713 +
1714 +static struct platform_driver bcm2835_isp_pdrv = {
1715 +       .probe = bcm2835_isp_probe,
1716 +       .remove = bcm2835_isp_remove,
1717 +       .driver = {
1718 +                       .name = BCM2835_ISP_NAME,
1719 +                 },
1720 +};
1721 +
1722 +module_platform_driver(bcm2835_isp_pdrv);
1723 +
1724 +MODULE_DESCRIPTION("BCM2835 ISP driver");
1725 +MODULE_AUTHOR("Naushir Patuck <naush@raspberrypi.com>");
1726 +MODULE_LICENSE("GPL");
1727 +MODULE_VERSION("1.0");
1728 +MODULE_ALIAS("platform:bcm2835-isp");
1729 --- /dev/null
1730 +++ b/drivers/staging/vc04_services/bcm2835-isp/bcm2835_isp_ctrls.h
1731 @@ -0,0 +1,67 @@
1732 +/* SPDX-License-Identifier: GPL-2.0 */
1733 +/*
1734 + * Broadcom BCM2835 ISP driver
1735 + *
1736 + * Copyright © 2019-2020 Raspberry Pi (Trading) Ltd.
1737 + *
1738 + * Author: Naushir Patuck (naush@raspberrypi.com)
1739 + *
1740 + */
1741 +
1742 +#ifndef BCM2835_ISP_CTRLS
1743 +#define BCM2835_ISP_CTRLS
1744 +
1745 +#include <linux/bcm2835-isp.h>
1746 +
1747 +struct bcm2835_isp_custom_ctrl {
1748 +       const char *name;
1749 +       u32 id;
1750 +       u32 size;
1751 +       u32 flags;
1752 +};
1753 +
1754 +static const struct bcm2835_isp_custom_ctrl custom_ctrls[] = {
1755 +       {
1756 +               .name   = "Colour Correction Matrix",
1757 +               .id     = V4L2_CID_USER_BCM2835_ISP_CC_MATRIX,
1758 +               .size   = sizeof(struct bcm2835_isp_custom_ccm),
1759 +               .flags  = 0
1760 +       }, {
1761 +               .name   = "Lens Shading",
1762 +               .id     = V4L2_CID_USER_BCM2835_ISP_LENS_SHADING,
1763 +               .size   = sizeof(struct bcm2835_isp_lens_shading),
1764 +               .flags  = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE
1765 +       }, {
1766 +               .name   = "Black Level",
1767 +               .id     = V4L2_CID_USER_BCM2835_ISP_BLACK_LEVEL,
1768 +               .size   = sizeof(struct bcm2835_isp_black_level),
1769 +               .flags  = 0
1770 +       }, {
1771 +               .name   = "Green Equalisation",
1772 +               .id     = V4L2_CID_USER_BCM2835_ISP_GEQ,
1773 +               .size   = sizeof(struct bcm2835_isp_geq),
1774 +               .flags  = 0
1775 +       }, {
1776 +               .name   = "Gamma",
1777 +               .id     = V4L2_CID_USER_BCM2835_ISP_GAMMA,
1778 +               .size   = sizeof(struct bcm2835_isp_gamma),
1779 +               .flags  = 0
1780 +       }, {
1781 +               .name   = "Sharpen",
1782 +               .id     = V4L2_CID_USER_BCM2835_ISP_SHARPEN,
1783 +               .size   = sizeof(struct bcm2835_isp_sharpen),
1784 +               .flags  = 0
1785 +       }, {
1786 +               .name   = "Denoise",
1787 +               .id     = V4L2_CID_USER_BCM2835_ISP_DENOISE,
1788 +               .size   = sizeof(struct bcm2835_isp_denoise),
1789 +               .flags  = 0
1790 +       }, {
1791 +               .name   = "Defective Pixel Correction",
1792 +               .id     = V4L2_CID_USER_BCM2835_ISP_DPC,
1793 +               .size   = sizeof(struct bcm2835_isp_dpc),
1794 +               .flags  = 0
1795 +       }
1796 +};
1797 +
1798 +#endif
1799 --- /dev/null
1800 +++ b/drivers/staging/vc04_services/bcm2835-isp/bcm2835_isp_fmts.h
1801 @@ -0,0 +1,272 @@
1802 +/* SPDX-License-Identifier: GPL-2.0 */
1803 +/*
1804 + * Broadcom BCM2835 ISP driver
1805 + *
1806 + * Copyright © 2019-2020 Raspberry Pi (Trading) Ltd.
1807 + *
1808 + * Author: Naushir Patuck (naush@raspberrypi.com)
1809 + *
1810 + */
1811 +
1812 +#ifndef BCM2835_ISP_FMTS
1813 +#define BCM2835_ISP_FMTS
1814 +
1815 +#include <linux/videodev2.h>
1816 +#include "vchiq-mmal/mmal-encodings.h"
1817 +
1818 +struct bcm2835_isp_fmt {
1819 +       u32 fourcc;
1820 +       int depth;
1821 +       int bytesperline_align;
1822 +       u32 flags;
1823 +       u32 mmal_fmt;
1824 +       int size_multiplier_x2;
1825 +       enum v4l2_colorspace colorspace;
1826 +};
1827 +
1828 +struct bcm2835_isp_fmt_list {
1829 +       struct bcm2835_isp_fmt *list;
1830 +       unsigned int num_entries;
1831 +};
1832 +
1833 +static const struct bcm2835_isp_fmt supported_formats[] = {
1834 +       {
1835 +               /* YUV formats */
1836 +               .fourcc             = V4L2_PIX_FMT_YUV420,
1837 +               .depth              = 8,
1838 +               .bytesperline_align = 32,
1839 +               .flags              = 0,
1840 +               .mmal_fmt           = MMAL_ENCODING_I420,
1841 +               .size_multiplier_x2 = 3,
1842 +               .colorspace         = V4L2_COLORSPACE_SMPTE170M,
1843 +       }, {
1844 +               .fourcc             = V4L2_PIX_FMT_YVU420,
1845 +               .depth              = 8,
1846 +               .bytesperline_align = 32,
1847 +               .flags              = 0,
1848 +               .mmal_fmt           = MMAL_ENCODING_YV12,
1849 +               .size_multiplier_x2 = 3,
1850 +               .colorspace         = V4L2_COLORSPACE_SMPTE170M,
1851 +       }, {
1852 +               .fourcc             = V4L2_PIX_FMT_NV12,
1853 +               .depth              = 8,
1854 +               .bytesperline_align = 32,
1855 +               .flags              = 0,
1856 +               .mmal_fmt           = MMAL_ENCODING_NV12,
1857 +               .size_multiplier_x2 = 3,
1858 +               .colorspace         = V4L2_COLORSPACE_SMPTE170M,
1859 +       }, {
1860 +               .fourcc             = V4L2_PIX_FMT_NV21,
1861 +               .depth              = 8,
1862 +               .bytesperline_align = 32,
1863 +               .flags              = 0,
1864 +               .mmal_fmt           = MMAL_ENCODING_NV21,
1865 +               .size_multiplier_x2 = 3,
1866 +               .colorspace         = V4L2_COLORSPACE_SMPTE170M,
1867 +       }, {
1868 +               .fourcc             = V4L2_PIX_FMT_YUYV,
1869 +               .depth              = 16,
1870 +               .bytesperline_align = 32,
1871 +               .flags              = 0,
1872 +               .mmal_fmt           = MMAL_ENCODING_YUYV,
1873 +               .size_multiplier_x2 = 2,
1874 +               .colorspace         = V4L2_COLORSPACE_SMPTE170M,
1875 +       }, {
1876 +               .fourcc             = V4L2_PIX_FMT_UYVY,
1877 +               .depth              = 16,
1878 +               .bytesperline_align = 32,
1879 +               .flags              = 0,
1880 +               .mmal_fmt           = MMAL_ENCODING_UYVY,
1881 +               .size_multiplier_x2 = 2,
1882 +               .colorspace         = V4L2_COLORSPACE_SMPTE170M,
1883 +       }, {
1884 +               .fourcc             = V4L2_PIX_FMT_YVYU,
1885 +               .depth              = 16,
1886 +               .bytesperline_align = 32,
1887 +               .flags              = 0,
1888 +               .mmal_fmt           = MMAL_ENCODING_YVYU,
1889 +               .size_multiplier_x2 = 2,
1890 +               .colorspace         = V4L2_COLORSPACE_SMPTE170M,
1891 +       }, {
1892 +               .fourcc             = V4L2_PIX_FMT_VYUY,
1893 +               .depth              = 16,
1894 +               .bytesperline_align = 32,
1895 +               .flags              = 0,
1896 +               .mmal_fmt           = MMAL_ENCODING_VYUY,
1897 +               .size_multiplier_x2 = 2,
1898 +               .colorspace         = V4L2_COLORSPACE_SMPTE170M,
1899 +       }, {
1900 +               /* RGB formats */
1901 +               .fourcc             = V4L2_PIX_FMT_RGB24,
1902 +               .depth              = 24,
1903 +               .bytesperline_align = 32,
1904 +               .flags              = 0,
1905 +               .mmal_fmt           = MMAL_ENCODING_RGB24,
1906 +               .size_multiplier_x2 = 2,
1907 +               .colorspace         = V4L2_COLORSPACE_SRGB,
1908 +       }, {
1909 +               .fourcc             = V4L2_PIX_FMT_RGB565,
1910 +               .depth              = 16,
1911 +               .bytesperline_align = 32,
1912 +               .flags              = 0,
1913 +               .mmal_fmt           = MMAL_ENCODING_RGB16,
1914 +               .size_multiplier_x2 = 2,
1915 +               .colorspace         = V4L2_COLORSPACE_SRGB,
1916 +       }, {
1917 +               .fourcc             = V4L2_PIX_FMT_BGR24,
1918 +               .depth              = 24,
1919 +               .bytesperline_align = 32,
1920 +               .flags              = 0,
1921 +               .mmal_fmt           = MMAL_ENCODING_BGR24,
1922 +               .size_multiplier_x2 = 2,
1923 +               .colorspace         = V4L2_COLORSPACE_SRGB,
1924 +       }, {
1925 +               .fourcc             = V4L2_PIX_FMT_ABGR32,
1926 +               .depth              = 32,
1927 +               .bytesperline_align = 32,
1928 +               .flags              = 0,
1929 +               .mmal_fmt           = MMAL_ENCODING_BGRA,
1930 +               .size_multiplier_x2 = 2,
1931 +               .colorspace         = V4L2_COLORSPACE_SRGB,
1932 +       }, {
1933 +               /* Bayer formats */
1934 +               /* 8 bit */
1935 +               .fourcc             = V4L2_PIX_FMT_SRGGB8,
1936 +               .depth              = 8,
1937 +               .bytesperline_align = 32,
1938 +               .flags              = 0,
1939 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SRGGB8,
1940 +               .size_multiplier_x2 = 2,
1941 +               .colorspace         = V4L2_COLORSPACE_RAW,
1942 +       }, {
1943 +               .fourcc             = V4L2_PIX_FMT_SBGGR8,
1944 +               .depth              = 8,
1945 +               .bytesperline_align = 32,
1946 +               .flags              = 0,
1947 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SBGGR8,
1948 +               .size_multiplier_x2 = 2,
1949 +               .colorspace         = V4L2_COLORSPACE_RAW,
1950 +       }, {
1951 +               .fourcc             = V4L2_PIX_FMT_SGRBG8,
1952 +               .depth              = 8,
1953 +               .bytesperline_align = 32,
1954 +               .flags              = 0,
1955 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SGRBG8,
1956 +               .size_multiplier_x2 = 2,
1957 +               .colorspace         = V4L2_COLORSPACE_RAW,
1958 +       }, {
1959 +               .fourcc             = V4L2_PIX_FMT_SGBRG8,
1960 +               .depth              = 8,
1961 +               .bytesperline_align = 32,
1962 +               .flags              = 0,
1963 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SGBRG8,
1964 +               .size_multiplier_x2 = 2,
1965 +               .colorspace         = V4L2_COLORSPACE_RAW,
1966 +       }, {
1967 +               /* 10 bit */
1968 +               .fourcc             = V4L2_PIX_FMT_SRGGB10P,
1969 +               .depth              = 10,
1970 +               .bytesperline_align = 32,
1971 +               .flags              = 0,
1972 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SRGGB10P,
1973 +               .size_multiplier_x2 = 2,
1974 +               .colorspace         = V4L2_COLORSPACE_RAW,
1975 +       }, {
1976 +               .fourcc             = V4L2_PIX_FMT_SBGGR10P,
1977 +               .depth              = 10,
1978 +               .bytesperline_align = 32,
1979 +               .flags              = 0,
1980 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SBGGR10P,
1981 +               .size_multiplier_x2 = 2,
1982 +               .colorspace         = V4L2_COLORSPACE_RAW,
1983 +       }, {
1984 +               .fourcc             = V4L2_PIX_FMT_SGRBG10P,
1985 +               .depth              = 10,
1986 +               .bytesperline_align = 32,
1987 +               .flags              = 0,
1988 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SGRBG10P,
1989 +               .size_multiplier_x2 = 2,
1990 +               .colorspace         = V4L2_COLORSPACE_RAW,
1991 +       }, {
1992 +               .fourcc             = V4L2_PIX_FMT_SGBRG10P,
1993 +               .depth              = 10,
1994 +               .bytesperline_align = 32,
1995 +               .flags              = 0,
1996 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SGBRG10P,
1997 +               .size_multiplier_x2 = 2,
1998 +               .colorspace         = V4L2_COLORSPACE_RAW,
1999 +       }, {
2000 +               /* 12 bit */
2001 +               .fourcc             = V4L2_PIX_FMT_SRGGB12P,
2002 +               .depth              = 12,
2003 +               .bytesperline_align = 32,
2004 +               .flags              = 0,
2005 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SRGGB12P,
2006 +               .size_multiplier_x2 = 2,
2007 +               .colorspace         = V4L2_COLORSPACE_RAW,
2008 +       }, {
2009 +               .fourcc             = V4L2_PIX_FMT_SBGGR12P,
2010 +               .depth              = 12,
2011 +               .bytesperline_align = 32,
2012 +               .flags              = 0,
2013 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SBGGR12P,
2014 +               .size_multiplier_x2 = 2,
2015 +               .colorspace         = V4L2_COLORSPACE_RAW,
2016 +       }, {
2017 +               .fourcc             = V4L2_PIX_FMT_SGRBG12P,
2018 +               .depth              = 12,
2019 +               .bytesperline_align = 32,
2020 +               .flags              = 0,
2021 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SGRBG12P,
2022 +               .size_multiplier_x2 = 2,
2023 +               .colorspace         = V4L2_COLORSPACE_RAW,
2024 +       }, {
2025 +               .fourcc             = V4L2_PIX_FMT_SGBRG12P,
2026 +               .depth              = 12,
2027 +               .bytesperline_align = 32,
2028 +               .flags              = 0,
2029 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SGBRG12P,
2030 +               .size_multiplier_x2 = 2,
2031 +               .colorspace         = V4L2_COLORSPACE_RAW,
2032 +       }, {
2033 +               /* 16 bit */
2034 +               .fourcc             = V4L2_PIX_FMT_SRGGB16,
2035 +               .depth              = 16,
2036 +               .bytesperline_align = 32,
2037 +               .flags              = 0,
2038 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SRGGB16,
2039 +               .size_multiplier_x2 = 2,
2040 +               .colorspace         = V4L2_COLORSPACE_RAW,
2041 +       }, {
2042 +               .fourcc             = V4L2_PIX_FMT_SBGGR16,
2043 +               .depth              = 16,
2044 +               .bytesperline_align = 32,
2045 +               .flags              = 0,
2046 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SBGGR16,
2047 +               .size_multiplier_x2 = 2,
2048 +               .colorspace         = V4L2_COLORSPACE_RAW,
2049 +       }, {
2050 +               .fourcc             = V4L2_PIX_FMT_SGRBG16,
2051 +               .depth              = 16,
2052 +               .bytesperline_align = 32,
2053 +               .flags              = 0,
2054 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SGRBG16,
2055 +               .size_multiplier_x2 = 2,
2056 +               .colorspace         = V4L2_COLORSPACE_RAW,
2057 +       }, {
2058 +               .fourcc             = V4L2_PIX_FMT_SGBRG16,
2059 +               .depth              = 16,
2060 +               .bytesperline_align = 32,
2061 +               .flags              = 0,
2062 +               .mmal_fmt           = MMAL_ENCODING_BAYER_SGBRG16,
2063 +               .size_multiplier_x2 = 2,
2064 +               .colorspace         = V4L2_COLORSPACE_RAW,
2065 +       }, {
2066 +               /* ISP statistics format */
2067 +               .fourcc             = V4L2_META_FMT_BCM2835_ISP_STATS,
2068 +               .mmal_fmt           = MMAL_ENCODING_BRCM_STATS,
2069 +               /* The rest are not valid fields for stats. */
2070 +       }
2071 +};
2072 +
2073 +#endif
2074 --- a/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h
2075 +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h
2076 @@ -100,6 +100,10 @@
2077   */
2078  #define MMAL_ENCODING_EGL_IMAGE        MMAL_FOURCC('E', 'G', 'L', 'I')
2079
2080 +/** ISP image statistics format
2081 + */
2082 +#define MMAL_ENCODING_BRCM_STATS       MMAL_FOURCC('S', 'T', 'A', 'T')
2083 +
2084  /* }@ */
2085
2086  /** \name Pre-defined audio encodings */
2087 --- a/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h
2088 +++ b/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h
2089 @@ -221,6 +221,62 @@ enum mmal_parameter_camera_type {
2090         MMAL_PARAMETER_SHUTTER_SPEED,
2091                 /**< Takes a @ref MMAL_PARAMETER_AWB_GAINS_T */
2092         MMAL_PARAMETER_CUSTOM_AWB_GAINS,
2093 +               /**< Takes a @ref MMAL_PARAMETER_CAMERA_SETTINGS_T */
2094 +       MMAL_PARAMETER_CAMERA_SETTINGS,
2095 +               /**< Takes a @ref MMAL_PARAMETER_PRIVACY_INDICATOR_T */
2096 +       MMAL_PARAMETER_PRIVACY_INDICATOR,
2097 +               /**< Takes a @ref MMAL_PARAMETER_BOOLEAN_T */
2098 +       MMAL_PARAMETER_VIDEO_DENOISE,
2099 +               /**< Takes a @ref MMAL_PARAMETER_BOOLEAN_T */
2100 +       MMAL_PARAMETER_STILLS_DENOISE,
2101 +               /**< Takes a @ref MMAL_PARAMETER_CAMERA_ANNOTATE_T */
2102 +       MMAL_PARAMETER_ANNOTATE,
2103 +               /**< Takes a @ref MMAL_PARAMETER_STEREOSCOPIC_MODE_T */
2104 +       MMAL_PARAMETER_STEREOSCOPIC_MODE,
2105 +               /**< Takes a @ref MMAL_PARAMETER_CAMERA_INTERFACE_T */
2106 +       MMAL_PARAMETER_CAMERA_INTERFACE,
2107 +               /**< Takes a @ref MMAL_PARAMETER_CAMERA_CLOCKING_MODE_T */
2108 +       MMAL_PARAMETER_CAMERA_CLOCKING_MODE,
2109 +               /**< Takes a @ref MMAL_PARAMETER_CAMERA_RX_CONFIG_T */
2110 +       MMAL_PARAMETER_CAMERA_RX_CONFIG,
2111 +               /**< Takes a @ref MMAL_PARAMETER_CAMERA_RX_TIMING_T */
2112 +       MMAL_PARAMETER_CAMERA_RX_TIMING,
2113 +               /**< Takes a @ref MMAL_PARAMETER_UINT32_T */
2114 +       MMAL_PARAMETER_DPF_CONFIG,
2115 +
2116 +       /* 0x50 */
2117 +               /**< Takes a @ref MMAL_PARAMETER_UINT32_T */
2118 +       MMAL_PARAMETER_JPEG_RESTART_INTERVAL,
2119 +               /**< Takes a @ref MMAL_PARAMETER_UINT32_T */
2120 +       MMAL_PARAMETER_CAMERA_ISP_BLOCK_OVERRIDE,
2121 +               /**< Takes a @ref MMAL_PARAMETER_LENS_SHADING_T */
2122 +       MMAL_PARAMETER_LENS_SHADING_OVERRIDE,
2123 +               /**< Takes a @ref MMAL_PARAMETER_UINT32_T */
2124 +       MMAL_PARAMETER_BLACK_LEVEL,
2125 +               /**< Takes a @ref MMAL_PARAMETER_RESIZE_T */
2126 +       MMAL_PARAMETER_RESIZE_PARAMS,
2127 +               /**< Takes a @ref MMAL_PARAMETER_CROP_T */
2128 +       MMAL_PARAMETER_CROP,
2129 +               /**< Takes a @ref MMAL_PARAMETER_INT32_T */
2130 +       MMAL_PARAMETER_OUTPUT_SHIFT,
2131 +               /**< Takes a @ref MMAL_PARAMETER_INT32_T */
2132 +       MMAL_PARAMETER_CCM_SHIFT,
2133 +               /**< Takes a @ref MMAL_PARAMETER_CUSTOM_CCM_T */
2134 +       MMAL_PARAMETER_CUSTOM_CCM,
2135 +               /**< Takes a @ref MMAL_PARAMETER_RATIONAL_T */
2136 +       MMAL_PARAMETER_ANALOG_GAIN,
2137 +               /**< Takes a @ref MMAL_PARAMETER_RATIONAL_T */
2138 +       MMAL_PARAMETER_DIGITAL_GAIN,
2139 +               /**< Takes a @ref MMAL_PARAMETER_DENOISE_T */
2140 +       MMAL_PARAMETER_DENOISE,
2141 +               /**< Takes a @ref MMAL_PARAMETER_SHARPEN_T */
2142 +       MMAL_PARAMETER_SHARPEN,
2143 +               /**< Takes a @ref MMAL_PARAMETER_GEQ_T */
2144 +       MMAL_PARAMETER_GEQ,
2145 +               /**< Tales a @ref MMAP_PARAMETER_DPC_T */
2146 +       MMAL_PARAMETER_DPC,
2147 +               /**< Tales a @ref MMAP_PARAMETER_GAMMA_T */
2148 +       MMAL_PARAMETER_GAMMA,
2149  };
2150
2151  struct mmal_parameter_rational {
2152 @@ -780,7 +836,102 @@ struct mmal_parameter_camera_info {
2153         struct mmal_parameter_camera_info_camera
2154                 cameras[MMAL_PARAMETER_CAMERA_INFO_MAX_CAMERAS];
2155         struct mmal_parameter_camera_info_flash
2156 -                               flashes[MMAL_PARAMETER_CAMERA_INFO_MAX_FLASHES];
2157 +               flashes[MMAL_PARAMETER_CAMERA_INFO_MAX_FLASHES];
2158 +};
2159 +
2160 +struct mmal_parameter_ccm {
2161 +       struct mmal_parameter_rational ccm[3][3];
2162 +       s32 offsets[3];
2163 +};
2164 +
2165 +struct mmal_parameter_custom_ccm {
2166 +       u32 enabled; /**< Enable the custom CCM. */
2167 +       struct mmal_parameter_ccm ccm; /**< CCM to be used. */
2168 +};
2169 +
2170 +struct mmal_parameter_lens_shading {
2171 +       u32 enabled;
2172 +       u32 grid_cell_size;
2173 +       u32 grid_width;
2174 +       u32 grid_stride;
2175 +       u32 grid_height;
2176 +       u32 mem_handle_table;
2177 +       u32 ref_transform;
2178 +};
2179 +
2180 +enum mmal_parameter_ls_gain_format_type {
2181 +       MMAL_PARAMETER_LS_GAIN_FORMAT_TYPE_U0P8_1 = 0,
2182 +       MMAL_PARAMETER_LS_GAIN_FORMAT_TYPE_U1P7_0 = 1,
2183 +       MMAL_PARAMETER_LS_GAIN_FORMAT_TYPE_U1P7_1 = 2,
2184 +       MMAL_PARAMETER_LS_GAIN_FORMAT_TYPE_U2P6_0 = 3,
2185 +       MMAL_PARAMETER_LS_GAIN_FORMAT_TYPE_U2P6_1 = 4,
2186 +       MMAL_PARAMETER_LS_GAIN_FORMAT_TYPE_U3P5_0 = 5,
2187 +       MMAL_PARAMETER_LS_GAIN_FORMAT_TYPE_U3P5_1 = 6,
2188 +       MMAL_PARAMETER_LS_GAIN_FORMAT_TYPE_U4P10  = 7,
2189 +       MMAL_PARAMETER_LS_GAIN_FORMAT_TYPE_DUMMY  = 0x7FFFFFFF
2190 +};
2191 +
2192 +struct mmal_parameter_lens_shading_v2 {
2193 +       u32 enabled;
2194 +       u32 grid_cell_size;
2195 +       u32 grid_width;
2196 +       u32 grid_stride;
2197 +       u32 grid_height;
2198 +       u32 mem_handle_table;
2199 +       u32 ref_transform;
2200 +       u32 corner_sampled;
2201 +       enum mmal_parameter_ls_gain_format_type gain_format;
2202 +};
2203 +
2204 +struct mmal_parameter_black_level {
2205 +       u32 enabled;
2206 +       u16 black_level_r;
2207 +       u16 black_level_g;
2208 +       u16 black_level_b;
2209 +       u8 pad_[2]; /* Unused */
2210 +};
2211 +
2212 +struct mmal_parameter_geq {
2213 +       u32 enabled;
2214 +       u32 offset;
2215 +       struct mmal_parameter_rational slope;
2216 +};
2217 +
2218 +#define MMAL_NUM_GAMMA_PTS 33
2219 +struct mmal_parameter_gamma {
2220 +       u32 enabled;
2221 +       u16 x[MMAL_NUM_GAMMA_PTS];
2222 +       u16 y[MMAL_NUM_GAMMA_PTS];
2223 +};
2224 +
2225 +struct mmal_parameter_denoise {
2226 +       u32 enabled;
2227 +       u32 constant;
2228 +       struct mmal_parameter_rational slope;
2229 +       struct mmal_parameter_rational strength;
2230 +};
2231 +
2232 +struct mmal_parameter_sharpen {
2233 +       u32 enabled;
2234 +       struct mmal_parameter_rational threshold;
2235 +       struct mmal_parameter_rational strength;
2236 +       struct mmal_parameter_rational limit;
2237 +};
2238 +
2239 +enum mmal_dpc_mode {
2240 +       MMAL_DPC_MODE_OFF = 0,
2241 +       MMAL_DPC_MODE_NORMAL = 1,
2242 +       MMAL_DPC_MODE_STRONG = 2,
2243 +       MMAL_DPC_MODE_MAX = 0x7FFFFFFF,
2244 +};
2245 +
2246 +struct mmal_parameter_dpc {
2247 +       u32 enabled;
2248 +       u32 strength;
2249 +};
2250 +
2251 +struct mmal_parameter_crop {
2252 +       struct vchiq_mmal_rect rect;
2253  };
2254
2255  #endif