drivers/staging/vc04_services/bcm2835-isp/bcm2835-v4l2-isp.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Broadcom BCM2835 ISP driver
   4  *
   5  * Copyright © 2019-2020 Raspberry Pi (Trading) Ltd.
   6  *
   7  * Author: Naushir Patuck (naush@raspberrypi.com)
   8  *
   9  */
  10
  11 #include <linux/module.h>
  12 #include <linux/platform_device.h>
  13
  14 #include <media/v4l2-ctrls.h>
  15 #include <media/v4l2-device.h>
  16 #include <media/v4l2-event.h>
  17 #include <media/v4l2-ioctl.h>
  18 #include <media/videobuf2-dma-contig.h>
  19
  20 #include "vchiq-mmal/mmal-msg.h"
  21 #include "vchiq-mmal/mmal-parameters.h"
  22 #include "vchiq-mmal/mmal-vchiq.h"
  23
  24 #include "vc-sm-cma/vc_sm_knl.h"
  25
  26 #include "bcm2835-isp-ctrls.h"
  27 #include "bcm2835-isp-fmts.h"
  28
  29 /*
  30  * We want to instantiate 2 independent instances allowing 2 simultaneous users
  31  * of the ISP hardware.
  32  */
  33 #define BCM2835_ISP_NUM_INSTANCES 2
  34
  35 static unsigned int debug;
  36 module_param(debug, uint, 0644);
  37 MODULE_PARM_DESC(debug, "activates debug info");
  38
  39 static unsigned int video_nr[BCM2835_ISP_NUM_INSTANCES] = { 13, 20 };
  40 module_param_array(video_nr, uint, NULL, 0644);
  41 MODULE_PARM_DESC(video_nr, "base video device numbers");
  42
  43 #define BCM2835_ISP_NAME "bcm2835-isp"
  44 #define BCM2835_ISP_ENTITY_NAME_LEN 32
  45
  46 #define BCM2835_ISP_NUM_OUTPUTS 1
  47 #define BCM2835_ISP_NUM_CAPTURES 2
  48 #define BCM2835_ISP_NUM_METADATA 1
  49
  50 #define BCM2835_ISP_NUM_NODES                                           \
  51                 (BCM2835_ISP_NUM_OUTPUTS + BCM2835_ISP_NUM_CAPTURES +   \
  52                  BCM2835_ISP_NUM_METADATA)
  53
  54 /* Default frame dimension of 1280 pixels. */
  55 #define DEFAULT_DIM 1280U
  56 /*
  57  * Maximum frame dimension of 16384 pixels.  Even though the ISP runs in tiles,
  58  * have a sensible limit so that we do not create an excessive number of tiles
  59  * to process.
  60  */
  61 #define MAX_DIM 16384U
  62 /*
  63  * Minimum frame dimension of 64 pixels.  Anything lower, and the tiling
  64  * algorithm may not be able to cope when applying filter context.
  65  */
  66 #define MIN_DIM 64U
  67
  68 /* Timeout for stop_streaming to allow all buffers to return */
  69 #define COMPLETE_TIMEOUT (2 * HZ)
  70
  71 /* Per-queue, driver-specific private data */
  72 struct bcm2835_isp_q_data {
  73         /*
  74          * These parameters should be treated as gospel, with everything else
  75          * being determined from them.
  76          */
  77         unsigned int bytesperline;
  78         unsigned int width;
  79         unsigned int height;
  80         unsigned int sizeimage;
  81         enum v4l2_colorspace colorspace;
  82         const struct bcm2835_isp_fmt *fmt;
  83 };
  84
  85 /*
  86  * Structure to describe a single node /dev/video<N> which represents a single
  87  * input or output queue to the ISP device.
  88  */
  89 struct bcm2835_isp_node {
  90         int vfl_dir;
  91         unsigned int id;
  92         const char *name;
  93         struct vchiq_mmal_port *port;
  94         struct video_device vfd;
  95         struct media_pad pad;
  96         struct media_intf_devnode *intf_devnode;
  97         struct media_link *intf_link;
  98         struct mutex lock; /* top level device node lock */
  99         struct mutex queue_lock;
 100
 101         struct vb2_queue queue;
 102         unsigned int sequence;
 103
 104         /* The list of formats supported on the node. */
 105         struct bcm2835_isp_fmt const **supported_fmts;
 106         unsigned int num_supported_fmts;
 107
 108         struct bcm2835_isp_q_data q_data;
 109
 110         /* Parent device structure */
 111         struct bcm2835_isp_dev *dev;
 112
 113         bool registered;
 114         bool media_node_registered;
 115 };
 116
 117 /*
 118  * Structure representing the entire ISP device, comprising several input and
 119  * output nodes /dev/video<N>.
 120  */
 121 struct bcm2835_isp_dev {
 122         struct v4l2_device v4l2_dev;
 123         struct device *dev;
 124         struct v4l2_ctrl_handler ctrl_handler;
 125         struct media_device mdev;
 126         struct media_entity entity;
 127         bool media_device_registered;
 128         bool media_entity_registered;
 129         struct vchiq_mmal_instance *mmal_instance;
 130         struct vchiq_mmal_component *component;
 131         struct completion frame_cmplt;
 132
 133         struct bcm2835_isp_node node[BCM2835_ISP_NUM_NODES];
 134         struct media_pad pad[BCM2835_ISP_NUM_NODES];
 135         atomic_t num_streaming;
 136
 137         /* Image pipeline controls. */
 138         int r_gain;
 139         int b_gain;
 140 };
 141
 142 struct bcm2835_isp_buffer {
 143         struct vb2_v4l2_buffer vb;
 144         struct mmal_buffer mmal;
 145 };
 146
 147 static
 148 inline struct bcm2835_isp_dev *node_get_dev(struct bcm2835_isp_node *node)
 149 {
 150         return node->dev;
 151 }
 152
 153 static inline bool node_is_output(struct bcm2835_isp_node *node)
 154 {
 155         return node->queue.type == V4L2_BUF_TYPE_VIDEO_OUTPUT;
 156 }
 157
 158 static inline bool node_is_capture(struct bcm2835_isp_node *node)
 159 {
 160         return node->queue.type == V4L2_BUF_TYPE_VIDEO_CAPTURE;
 161 }
 162
 163 static inline bool node_is_stats(struct bcm2835_isp_node *node)
 164 {
 165         return node->queue.type == V4L2_BUF_TYPE_META_CAPTURE;
 166 }
 167
 168 static inline enum v4l2_buf_type index_to_queue_type(int index)
 169 {
 170         if (index < BCM2835_ISP_NUM_OUTPUTS)
 171                 return V4L2_BUF_TYPE_VIDEO_OUTPUT;
 172         else if (index < BCM2835_ISP_NUM_OUTPUTS + BCM2835_ISP_NUM_CAPTURES)
 173                 return V4L2_BUF_TYPE_VIDEO_CAPTURE;
 174         else
 175                 return V4L2_BUF_TYPE_META_CAPTURE;
 176 }
 177
 178 static int set_isp_param(struct bcm2835_isp_node *node, u32 parameter,
 179                          void *value, u32 value_size)
 180 {
 181         struct bcm2835_isp_dev *dev = node_get_dev(node);
 182
 183         return vchiq_mmal_port_parameter_set(dev->mmal_instance, node->port,
 184                                              parameter, value, value_size);
 185 }
 186
 187 static int set_wb_gains(struct bcm2835_isp_node *node)
 188 {
 189         struct bcm2835_isp_dev *dev = node_get_dev(node);
 190         struct mmal_parameter_awbgains gains = {
 191                 .r_gain = { dev->r_gain, 1000 },
 192                 .b_gain = { dev->b_gain, 1000 }
 193         };
 194
 195         return set_isp_param(node, MMAL_PARAMETER_CUSTOM_AWB_GAINS,
 196                              &gains, sizeof(gains));
 197 }
 198
 199 static int set_digital_gain(struct bcm2835_isp_node *node, uint32_t gain)
 200 {
 201         struct mmal_parameter_rational digital_gain = {
 202                 .num = gain,
 203                 .den = 1000
 204         };
 205
 206         return set_isp_param(node, MMAL_PARAMETER_DIGITAL_GAIN,
 207                              &digital_gain, sizeof(digital_gain));
 208 }
 209
 210 static const struct bcm2835_isp_fmt *get_fmt(u32 mmal_fmt)
 211 {
 212         unsigned int i;
 213
 214         for (i = 0; i < ARRAY_SIZE(supported_formats); i++) {
 215                 if (supported_formats[i].mmal_fmt == mmal_fmt)
 216                         return &supported_formats[i];
 217         }
 218         return NULL;
 219 }
 220
 221 static const
 222 struct bcm2835_isp_fmt *find_format_by_fourcc(unsigned int fourcc,
 223                                               struct bcm2835_isp_node *node)
 224 {
 225         const struct bcm2835_isp_fmt *fmt;
 226         unsigned int i;
 227
 228         for (i = 0; i < node->num_supported_fmts; i++) {
 229                 fmt = node->supported_fmts[i];
 230                 if (fmt->fourcc == fourcc)
 231                         return fmt;
 232         }
 233
 234         return NULL;
 235 }
 236
 237 static const
 238 struct bcm2835_isp_fmt *find_format(struct v4l2_format *f,
 239                                     struct bcm2835_isp_node *node)
 240 {
 241         return find_format_by_fourcc(node_is_stats(node) ?
 242                                      f->fmt.meta.dataformat :
 243                                      f->fmt.pix.pixelformat,
 244                                      node);
 245 }
 246
 247 /* vb2_to_mmal_buffer() - converts vb2 buffer header to MMAL
 248  *
 249  * Copies all the required fields from a VB2 buffer to the MMAL buffer header,
 250  * ready for sending to the VPU.
 251  */
 252 static void vb2_to_mmal_buffer(struct mmal_buffer *buf,
 253                                struct vb2_v4l2_buffer *vb2)
 254 {
 255         u64 pts;
 256
 257         buf->mmal_flags = 0;
 258         if (vb2->flags & V4L2_BUF_FLAG_KEYFRAME)
 259                 buf->mmal_flags |= MMAL_BUFFER_HEADER_FLAG_KEYFRAME;
 260
 261         /* Data must be framed correctly as one frame per buffer. */
 262         buf->mmal_flags |= MMAL_BUFFER_HEADER_FLAG_FRAME_END;
 263
 264         buf->length = vb2->vb2_buf.planes[0].bytesused;
 265         /*
 266          * Minor ambiguity in the V4L2 spec as to whether passing in a 0 length
 267          * buffer, or one with V4L2_BUF_FLAG_LAST set denotes end of stream.
 268          * Handle either.
 269          */
 270         if (!buf->length || vb2->flags & V4L2_BUF_FLAG_LAST)
 271                 buf->mmal_flags |= MMAL_BUFFER_HEADER_FLAG_EOS;
 272
 273         /* vb2 timestamps in nsecs, mmal in usecs */
 274         pts = vb2->vb2_buf.timestamp;
 275         do_div(pts, 1000);
 276         buf->pts = pts;
 277         buf->dts = MMAL_TIME_UNKNOWN;
 278 }
 279
 280 static void mmal_buffer_cb(struct vchiq_mmal_instance *instance,
 281                            struct vchiq_mmal_port *port, int status,
 282                            struct mmal_buffer *mmal_buf)
 283 {
 284         struct bcm2835_isp_buffer *q_buf;
 285         struct bcm2835_isp_node *node = port->cb_ctx;
 286         struct bcm2835_isp_dev *dev = node_get_dev(node);
 287         struct vb2_v4l2_buffer *vb2;
 288
 289         q_buf = container_of(mmal_buf, struct bcm2835_isp_buffer, mmal);
 290         vb2 = &q_buf->vb;
 291         v4l2_dbg(2, debug, &dev->v4l2_dev,
 292                  "%s: port:%s[%d], status:%d, buf:%p, dmabuf:%p, length:%lu, flags %u, pts %lld\n",
 293                  __func__, node_is_output(node) ? "input" : "output", node->id,
 294                  status, mmal_buf, mmal_buf->dma_buf, mmal_buf->length,
 295                  mmal_buf->mmal_flags, mmal_buf->pts);
 296
 297         if (mmal_buf->cmd)
 298                 v4l2_err(&dev->v4l2_dev,
 299                          "%s: Unexpected event on output callback - %08x\n",
 300                          __func__, mmal_buf->cmd);
 301
 302         if (status) {
 303                 /* error in transfer */
 304                 if (vb2) {
 305                         /* there was a buffer with the error so return it */
 306                         vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_ERROR);
 307                 }
 308                 return;
 309         }
 310
 311         /* vb2 timestamps in nsecs, mmal in usecs */
 312         vb2->vb2_buf.timestamp = mmal_buf->pts * 1000;
 313         vb2->sequence = node->sequence++;
 314         vb2_set_plane_payload(&vb2->vb2_buf, 0, mmal_buf->length);
 315         vb2_buffer_done(&vb2->vb2_buf, VB2_BUF_STATE_DONE);
 316
 317         if (!port->enabled)
 318                 complete(&dev->frame_cmplt);
 319 }
 320
 321 struct colorspace_translation {
 322         enum v4l2_colorspace v4l2_value;
 323         u32 mmal_value;
 324 };
 325
 326 static u32 translate_color_space(enum v4l2_colorspace color_space)
 327 {
 328         static const struct colorspace_translation translations[] = {
 329                 { V4L2_COLORSPACE_DEFAULT, MMAL_COLOR_SPACE_UNKNOWN },
 330                 { V4L2_COLORSPACE_SMPTE170M, MMAL_COLOR_SPACE_ITUR_BT601 },
 331                 { V4L2_COLORSPACE_SMPTE240M, MMAL_COLOR_SPACE_SMPTE240M },
 332                 { V4L2_COLORSPACE_REC709, MMAL_COLOR_SPACE_ITUR_BT709 },
 333                 /* V4L2_COLORSPACE_BT878 unavailable */
 334                 { V4L2_COLORSPACE_470_SYSTEM_M, MMAL_COLOR_SPACE_BT470_2_M },
 335                 { V4L2_COLORSPACE_470_SYSTEM_BG, MMAL_COLOR_SPACE_BT470_2_BG },
 336                 { V4L2_COLORSPACE_JPEG, MMAL_COLOR_SPACE_JPEG_JFIF },
 337                 /*
 338                  * We don't have an encoding for SRGB as such, but VideoCore
 339                  * will do the right thing if it gets "unknown".
 340                  */
 341                 { V4L2_COLORSPACE_SRGB, MMAL_COLOR_SPACE_UNKNOWN },
 342                 /* V4L2_COLORSPACE_OPRGB unavailable */
 343                 /* V4L2_COLORSPACE_BT2020 unavailable */
 344                 /* V4L2_COLORSPACE_RAW unavailable */
 345                 /* V4L2_COLORSPACE_DCI_P3 unavailable */
 346         };
 347
 348         unsigned int i;
 349
 350         for (i = 0; i < ARRAY_SIZE(translations); i++) {
 351                 if (color_space == translations[i].v4l2_value)
 352                         return translations[i].mmal_value;
 353         }
 354
 355         return MMAL_COLOR_SPACE_UNKNOWN;
 356 }
 357
 358 static void setup_mmal_port_format(struct bcm2835_isp_node *node,
 359                                    struct vchiq_mmal_port *port)
 360 {
 361         struct bcm2835_isp_q_data *q_data = &node->q_data;
 362
 363         port->format.encoding = q_data->fmt->mmal_fmt;
 364         /* Raw image format - set width/height */
 365         port->es.video.width = (q_data->bytesperline << 3) / q_data->fmt->depth;
 366         port->es.video.height = q_data->height;
 367         port->es.video.crop.width = q_data->width;
 368         port->es.video.crop.height = q_data->height;
 369         port->es.video.crop.x = 0;
 370         port->es.video.crop.y = 0;
 371         port->es.video.color_space = translate_color_space(q_data->colorspace);
 372 };
 373
 374 static int setup_mmal_port(struct bcm2835_isp_node *node)
 375 {
 376         struct bcm2835_isp_dev *dev = node_get_dev(node);
 377         unsigned int enable = 1;
 378         int ret;
 379
 380         v4l2_dbg(2, debug, &dev->v4l2_dev, "%s: setup %s[%d]\n", __func__,
 381                  node->name, node->id);
 382
 383         vchiq_mmal_port_parameter_set(dev->mmal_instance, node->port,
 384                                       MMAL_PARAMETER_ZERO_COPY, &enable,
 385                                       sizeof(enable));
 386         setup_mmal_port_format(node, node->port);
 387         ret = vchiq_mmal_port_set_format(dev->mmal_instance, node->port);
 388         if (ret < 0) {
 389                 v4l2_dbg(1, debug, &dev->v4l2_dev,
 390                          "%s: vchiq_mmal_port_set_format failed\n",
 391                          __func__);
 392                 return ret;
 393         }
 394
 395         if (node->q_data.sizeimage < node->port->minimum_buffer.size) {
 396                 v4l2_err(&dev->v4l2_dev,
 397                          "buffer size mismatch sizeimage %u < min size %u\n",
 398                          node->q_data.sizeimage,
 399                          node->port->minimum_buffer.size);
 400                 return -EINVAL;
 401         }
 402
 403         return 0;
 404 }
 405
 406 static int bcm2835_isp_mmal_buf_cleanup(struct mmal_buffer *mmal_buf)
 407 {
 408         mmal_vchi_buffer_cleanup(mmal_buf);
 409
 410         if (mmal_buf->dma_buf) {
 411                 dma_buf_put(mmal_buf->dma_buf);
 412                 mmal_buf->dma_buf = NULL;
 413         }
 414
 415         return 0;
 416 }
 417
 418 static int bcm2835_isp_node_queue_setup(struct vb2_queue *q,
 419                                         unsigned int *nbuffers,
 420                                         unsigned int *nplanes,
 421                                         unsigned int sizes[],
 422                                         struct device *alloc_devs[])
 423 {
 424         struct bcm2835_isp_node *node = vb2_get_drv_priv(q);
 425         unsigned int size;
 426
 427         if (setup_mmal_port(node))
 428                 return -EINVAL;
 429
 430         size = node->q_data.sizeimage;
 431         if (size == 0) {
 432                 v4l2_info(&node_get_dev(node)->v4l2_dev,
 433                           "%s: Image size unset in queue_setup for node %s[%d]\n",
 434                           __func__, node->name, node->id);
 435                 return -EINVAL;
 436         }
 437
 438         if (*nplanes)
 439                 return sizes[0] < size ? -EINVAL : 0;
 440
 441         *nplanes = 1;
 442         sizes[0] = size;
 443
 444         node->port->current_buffer.size = size;
 445
 446         if (*nbuffers < node->port->minimum_buffer.num)
 447                 *nbuffers = node->port->minimum_buffer.num;
 448
 449         node->port->current_buffer.num = *nbuffers;
 450
 451         v4l2_dbg(2, debug, &node_get_dev(node)->v4l2_dev,
 452                  "%s: Image size %u, nbuffers %u for node %s[%d]\n",
 453                  __func__, sizes[0], *nbuffers, node->name, node->id);
 454         return 0;
 455 }
 456
 457 static int bcm2835_isp_buf_init(struct vb2_buffer *vb)
 458 {
 459         struct bcm2835_isp_node *node = vb2_get_drv_priv(vb->vb2_queue);
 460         struct bcm2835_isp_dev *dev = node_get_dev(node);
 461         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 462         struct bcm2835_isp_buffer *buf =
 463                 container_of(vb2, struct bcm2835_isp_buffer, vb);
 464
 465         v4l2_dbg(3, debug, &dev->v4l2_dev, "%s: vb %p\n", __func__, vb);
 466
 467         buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
 468         buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
 469         mmal_vchi_buffer_init(dev->mmal_instance, &buf->mmal);
 470         return 0;
 471 }
 472
 473 static int bcm2835_isp_buf_prepare(struct vb2_buffer *vb)
 474 {
 475         struct bcm2835_isp_node *node = vb2_get_drv_priv(vb->vb2_queue);
 476         struct bcm2835_isp_dev *dev = node_get_dev(node);
 477         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 478         struct bcm2835_isp_buffer *buf =
 479                 container_of(vb2, struct bcm2835_isp_buffer, vb);
 480         struct dma_buf *dma_buf;
 481         int ret;
 482
 483         v4l2_dbg(3, debug, &dev->v4l2_dev, "%s: type: %d ptr %p\n",
 484                  __func__, vb->vb2_queue->type, vb);
 485
 486         if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
 487                 if (vb2->field == V4L2_FIELD_ANY)
 488                         vb2->field = V4L2_FIELD_NONE;
 489                 if (vb2->field != V4L2_FIELD_NONE) {
 490                         v4l2_err(&dev->v4l2_dev,
 491                                  "%s field isn't supported\n", __func__);
 492                         return -EINVAL;
 493                 }
 494         }
 495
 496         if (vb2_plane_size(vb, 0) < node->q_data.sizeimage) {
 497                 v4l2_err(&dev->v4l2_dev,
 498                          "%s data will not fit into plane (%lu < %lu)\n",
 499                          __func__, vb2_plane_size(vb, 0),
 500                          (long)node->q_data.sizeimage);
 501                 return -EINVAL;
 502         }
 503
 504         if (!V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type))
 505                 vb2_set_plane_payload(vb, 0, node->q_data.sizeimage);
 506
 507         switch (vb->memory) {
 508         case VB2_MEMORY_DMABUF:
 509                 dma_buf = dma_buf_get(vb->planes[0].m.fd);
 510
 511                 if (dma_buf != buf->mmal.dma_buf) {
 512                         /*
 513                          * dmabuf either hasn't already been mapped, or it has
 514                          * changed.
 515                          */
 516                         if (buf->mmal.dma_buf) {
 517                                 v4l2_err(&dev->v4l2_dev,
 518                                          "%s Buffer changed - why did the core not call cleanup?\n",
 519                                          __func__);
 520                                 bcm2835_isp_mmal_buf_cleanup(&buf->mmal);
 521                         }
 522
 523                         buf->mmal.dma_buf = dma_buf;
 524                 } else {
 525                         /*
 526                          * Already have a reference to the buffer, so release it
 527                          * here.
 528                          */
 529                         dma_buf_put(dma_buf);
 530                 }
 531                 ret = 0;
 532                 break;
 533         case VB2_MEMORY_MMAP:
 534                 /*
 535                  * We want to do this at init, but vb2_core_expbuf checks that
 536                  * the index < q->num_buffers, and q->num_buffers only gets
 537                  * updated once all the buffers are allocated.
 538                  */
 539                 if (!buf->mmal.dma_buf) {
 540                         ret = vb2_core_expbuf_dmabuf(vb->vb2_queue,
 541                                                      vb->vb2_queue->type,
 542                                                      vb->index, 0, O_CLOEXEC,
 543                                                      &buf->mmal.dma_buf);
 544                         v4l2_dbg(3, debug, &dev->v4l2_dev,
 545                                  "%s: exporting ptr %p to dmabuf %p\n",
 546                                  __func__, vb, buf->mmal.dma_buf);
 547                         if (ret)
 548                                 v4l2_err(&dev->v4l2_dev,
 549                                          "%s: Failed to expbuf idx %d, ret %d\n",
 550                                          __func__, vb->index, ret);
 551                 } else {
 552                         ret = 0;
 553                 }
 554                 break;
 555         default:
 556                 ret = -EINVAL;
 557                 break;
 558         }
 559
 560         return ret;
 561 }
 562
 563 static void bcm2835_isp_node_buffer_queue(struct vb2_buffer *buf)
 564 {
 565         struct bcm2835_isp_node *node = vb2_get_drv_priv(buf->vb2_queue);
 566         struct vb2_v4l2_buffer *vbuf =
 567                 container_of(buf, struct vb2_v4l2_buffer, vb2_buf);
 568         struct bcm2835_isp_buffer *buffer =
 569                 container_of(vbuf, struct bcm2835_isp_buffer, vb);
 570         struct bcm2835_isp_dev *dev = node_get_dev(node);
 571
 572         v4l2_dbg(3, debug, &dev->v4l2_dev, "%s: node %s[%d], buffer %p\n",
 573                  __func__, node->name, node->id, buffer);
 574
 575         vb2_to_mmal_buffer(&buffer->mmal, &buffer->vb);
 576         v4l2_dbg(3, debug, &dev->v4l2_dev,
 577                  "%s: node %s[%d] - submitting  mmal dmabuf %p\n", __func__,
 578                  node->name, node->id, buffer->mmal.dma_buf);
 579         vchiq_mmal_submit_buffer(dev->mmal_instance, node->port, &buffer->mmal);
 580 }
 581
 582 static void bcm2835_isp_buffer_cleanup(struct vb2_buffer *vb)
 583 {
 584         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
 585         struct bcm2835_isp_buffer *buffer =
 586                 container_of(vb2, struct bcm2835_isp_buffer, vb);
 587
 588         bcm2835_isp_mmal_buf_cleanup(&buffer->mmal);
 589 }
 590
 591 static int bcm2835_isp_node_start_streaming(struct vb2_queue *q,
 592                                             unsigned int count)
 593 {
 594         struct bcm2835_isp_node *node = vb2_get_drv_priv(q);
 595         struct bcm2835_isp_dev *dev = node_get_dev(node);
 596         int ret;
 597
 598         v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: node %s[%d] (count %u)\n",
 599                  __func__, node->name, node->id, count);
 600
 601         ret = vchiq_mmal_component_enable(dev->mmal_instance, dev->component);
 602         if (ret) {
 603                 v4l2_err(&dev->v4l2_dev, "%s: Failed enabling component, ret %d\n",
 604                          __func__, ret);
 605                 return -EIO;
 606         }
 607
 608         node->sequence = 0;
 609         node->port->cb_ctx = node;
 610         ret = vchiq_mmal_port_enable(dev->mmal_instance, node->port,
 611                                      mmal_buffer_cb);
 612         if (!ret)
 613                 atomic_inc(&dev->num_streaming);
 614         else
 615                 v4l2_err(&dev->v4l2_dev,
 616                          "%s: Failed enabling port, ret %d\n", __func__, ret);
 617
 618         return ret;
 619 }
 620
 621 static void bcm2835_isp_node_stop_streaming(struct vb2_queue *q)
 622 {
 623         struct bcm2835_isp_node *node = vb2_get_drv_priv(q);
 624         struct bcm2835_isp_dev *dev = node_get_dev(node);
 625         int ret;
 626
 627         v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: node %s[%d], mmal port %p\n",
 628                  __func__, node->name, node->id, node->port);
 629
 630         init_completion(&dev->frame_cmplt);
 631
 632         /* Disable MMAL port - this will flush buffers back */
 633         ret = vchiq_mmal_port_disable(dev->mmal_instance, node->port);
 634         if (ret)
 635                 v4l2_err(&dev->v4l2_dev,
 636                          "%s: Failed disabling %s port, ret %d\n", __func__,
 637                          node_is_output(node) ? "i/p" : "o/p",
 638                          ret);
 639
 640         while (atomic_read(&node->port->buffers_with_vpu)) {
 641                 v4l2_dbg(1, debug, &dev->v4l2_dev,
 642                          "%s: Waiting for buffers to be returned - %d outstanding\n",
 643                          __func__, atomic_read(&node->port->buffers_with_vpu));
 644                 ret = wait_for_completion_timeout(&dev->frame_cmplt,
 645                                                   COMPLETE_TIMEOUT);
 646                 if (ret <= 0) {
 647                         v4l2_err(&dev->v4l2_dev,
 648                                  "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
 649                                  __func__,
 650                                  atomic_read(&node->port->buffers_with_vpu));
 651                         break;
 652                 }
 653         }
 654
 655         atomic_dec(&dev->num_streaming);
 656         /* If all ports disabled, then disable the component */
 657         if (atomic_read(&dev->num_streaming) == 0) {
 658                 struct bcm2835_isp_lens_shading ls;
 659                 /*
 660                  * The ISP component on the firmware has a reference to the
 661                  * dmabuf handle for the lens shading table.  Pass a null handle
 662                  * to remove that reference now.
 663                  */
 664                 memset(&ls, 0, sizeof(ls));
 665                 /* Must set a valid grid size for the FW */
 666                 ls.grid_cell_size = 16;
 667                 set_isp_param(&dev->node[0],
 668                               MMAL_PARAMETER_LENS_SHADING_OVERRIDE,
 669                               &ls, sizeof(ls));
 670
 671                 ret = vchiq_mmal_component_disable(dev->mmal_instance,
 672                                                    dev->component);
 673                 if (ret) {
 674                         v4l2_err(&dev->v4l2_dev,
 675                                  "%s: Failed disabling component, ret %d\n",
 676                                  __func__, ret);
 677                 }
 678         }
 679
 680         /*
 681          * Simply wait for any vb2 buffers to finish. We could take steps to
 682          * make them complete more quickly if we care, or even return them
 683          * ourselves.
 684          */
 685         vb2_wait_for_all_buffers(&node->queue);
 686 }
 687
 688 static const struct vb2_ops bcm2835_isp_node_queue_ops = {
 689         .queue_setup            = bcm2835_isp_node_queue_setup,
 690         .buf_init               = bcm2835_isp_buf_init,
 691         .buf_prepare            = bcm2835_isp_buf_prepare,
 692         .buf_queue              = bcm2835_isp_node_buffer_queue,
 693         .buf_cleanup            = bcm2835_isp_buffer_cleanup,
 694         .start_streaming        = bcm2835_isp_node_start_streaming,
 695         .stop_streaming         = bcm2835_isp_node_stop_streaming,
 696 };
 697
 698 static const
 699 struct bcm2835_isp_fmt *get_default_format(struct bcm2835_isp_node *node)
 700 {
 701         return node->supported_fmts[0];
 702 }
 703
 704 static inline unsigned int get_bytesperline(int width,
 705                                             const struct bcm2835_isp_fmt *fmt)
 706 {
 707         /* GPU aligns 24bpp images to a multiple of 32 pixels (not bytes). */
 708         if (fmt->depth == 24)
 709                 return ALIGN(width, 32) * 3;
 710         else
 711                 return ALIGN((width * fmt->depth) >> 3, fmt->bytesperline_align);
 712 }
 713
 714 static inline unsigned int get_sizeimage(int bpl, int width, int height,
 715                                          const struct bcm2835_isp_fmt *fmt)
 716 {
 717         return (bpl * height * fmt->size_multiplier_x2) >> 1;
 718 }
 719
 720 static int bcm2835_isp_s_ctrl(struct v4l2_ctrl *ctrl)
 721 {
 722         struct bcm2835_isp_dev *dev =
 723               container_of(ctrl->handler, struct bcm2835_isp_dev, ctrl_handler);
 724         struct bcm2835_isp_node *node = &dev->node[0];
 725         int ret = 0;
 726
 727         /*
 728          * The ISP firmware driver will ensure these settings are applied on
 729          * a frame boundary, so we are safe to write them as they come in.
 730          *
 731          * Note that the bcm2835_isp_* param structures are identical to the
 732          * mmal-parameters.h definitions.  This avoids the need for unnecessary
 733          * field-by-field copying between structures.
 734          */
 735         switch (ctrl->id) {
 736         case V4L2_CID_RED_BALANCE:
 737                 dev->r_gain = ctrl->val;
 738                 ret = set_wb_gains(node);
 739                 break;
 740         case V4L2_CID_BLUE_BALANCE:
 741                 dev->b_gain = ctrl->val;
 742                 ret = set_wb_gains(node);
 743                 break;
 744         case V4L2_CID_DIGITAL_GAIN:
 745                 ret = set_digital_gain(node, ctrl->val);
 746                 break;
 747         case V4L2_CID_USER_BCM2835_ISP_CC_MATRIX:
 748                 ret = set_isp_param(node, MMAL_PARAMETER_CUSTOM_CCM,
 749                                     ctrl->p_new.p_u8,
 750                                     sizeof(struct bcm2835_isp_custom_ccm));
 751                 break;
 752         case V4L2_CID_USER_BCM2835_ISP_LENS_SHADING:
 753         {
 754                 struct bcm2835_isp_lens_shading *v4l2_ls;
 755                 struct mmal_parameter_lens_shading_v2 ls;
 756                 struct dma_buf *dmabuf;
 757                 void *vcsm_handle;
 758
 759                 v4l2_ls = (struct bcm2835_isp_lens_shading *)ctrl->p_new.p_u8;
 760                 /*
 761                  * struct bcm2835_isp_lens_shading and struct
 762                  * mmal_parameter_lens_shading_v2 match so that we can do a
 763                  * simple memcpy here.
 764                  * Only the dmabuf to the actual table needs any manipulation.
 765                  */
 766                 memcpy(&ls, v4l2_ls, sizeof(ls));
 767
 768                 dmabuf = dma_buf_get(v4l2_ls->dmabuf);
 769                 if (IS_ERR_OR_NULL(dmabuf))
 770                         return -EINVAL;
 771
 772                 ret = vc_sm_cma_import_dmabuf(dmabuf, &vcsm_handle);
 773                 if (ret) {
 774                         dma_buf_put(dmabuf);
 775                         return -EINVAL;
 776                 }
 777
 778                 ls.mem_handle_table = vc_sm_cma_int_handle(vcsm_handle);
 779                 if (ls.mem_handle_table)
 780                         /* The VPU will take a reference on the vcsm handle,
 781                          * which in turn will retain a reference on the dmabuf.
 782                          * This code can therefore safely release all
 783                          * references to the buffer.
 784                          */
 785                         ret = set_isp_param(node,
 786                                             MMAL_PARAMETER_LENS_SHADING_OVERRIDE,
 787                                             &ls,
 788                                             sizeof(ls));
 789                 else
 790                         ret = -EINVAL;
 791
 792                 vc_sm_cma_free(vcsm_handle);
 793                 dma_buf_put(dmabuf);
 794                 break;
 795         }
 796         case V4L2_CID_USER_BCM2835_ISP_BLACK_LEVEL:
 797                 ret = set_isp_param(node, MMAL_PARAMETER_BLACK_LEVEL,
 798                                     ctrl->p_new.p_u8,
 799                                     sizeof(struct bcm2835_isp_black_level));
 800                 break;
 801         case V4L2_CID_USER_BCM2835_ISP_GEQ:
 802                 ret = set_isp_param(node, MMAL_PARAMETER_GEQ,
 803                                     ctrl->p_new.p_u8,
 804                                     sizeof(struct bcm2835_isp_geq));
 805                 break;
 806         case V4L2_CID_USER_BCM2835_ISP_GAMMA:
 807                 ret = set_isp_param(node, MMAL_PARAMETER_GAMMA,
 808                                     ctrl->p_new.p_u8,
 809                                     sizeof(struct bcm2835_isp_gamma));
 810                 break;
 811         case V4L2_CID_USER_BCM2835_ISP_DENOISE:
 812                 ret = set_isp_param(node, MMAL_PARAMETER_DENOISE,
 813                                     ctrl->p_new.p_u8,
 814                                     sizeof(struct bcm2835_isp_denoise));
 815                 break;
 816         case V4L2_CID_USER_BCM2835_ISP_CDN:
 817                 ret = set_isp_param(node, MMAL_PARAMETER_CDN,
 818                                     ctrl->p_new.p_u8,
 819                                     sizeof(struct bcm2835_isp_cdn));
 820                 break;
 821         case V4L2_CID_USER_BCM2835_ISP_SHARPEN:
 822                 ret = set_isp_param(node, MMAL_PARAMETER_SHARPEN,
 823                                     ctrl->p_new.p_u8,
 824                                     sizeof(struct bcm2835_isp_sharpen));
 825                 break;
 826         case V4L2_CID_USER_BCM2835_ISP_DPC:
 827                 ret = set_isp_param(node, MMAL_PARAMETER_DPC,
 828                                     ctrl->p_new.p_u8,
 829                                     sizeof(struct bcm2835_isp_dpc));
 830                 break;
 831         default:
 832                 v4l2_info(&dev->v4l2_dev, "Unrecognised control\n");
 833                 ret = -EINVAL;
 834         }
 835
 836         if (ret) {
 837                 v4l2_err(&dev->v4l2_dev, "%s: Failed setting ctrl \"%s\" (%08x), err %d\n",
 838                          __func__, ctrl->name, ctrl->id, ret);
 839                 ret = -EIO;
 840         }
 841
 842         return ret;
 843 }
 844
 845 static const struct v4l2_ctrl_ops bcm2835_isp_ctrl_ops = {
 846         .s_ctrl = bcm2835_isp_s_ctrl,
 847 };
 848
 849 static const struct v4l2_file_operations bcm2835_isp_fops = {
 850         .owner          = THIS_MODULE,
 851         .open           = v4l2_fh_open,
 852         .release        = vb2_fop_release,
 853         .poll           = vb2_fop_poll,
 854         .unlocked_ioctl = video_ioctl2,
 855         .mmap           = vb2_fop_mmap
 856 };
 857
 858 static int populate_qdata_fmt(struct v4l2_format *f,
 859                               struct bcm2835_isp_node *node)
 860 {
 861         struct bcm2835_isp_dev *dev = node_get_dev(node);
 862         struct bcm2835_isp_q_data *q_data = &node->q_data;
 863         int ret;
 864
 865         if (!node_is_stats(node)) {
 866                 v4l2_dbg(1, debug, &dev->v4l2_dev,
 867                          "%s: Setting pix format for type %d, wxh: %ux%u, fmt: %08x, size %u\n",
 868                          __func__, f->type, f->fmt.pix.width, f->fmt.pix.height,
 869                          f->fmt.pix.pixelformat, f->fmt.pix.sizeimage);
 870
 871                 q_data->fmt = find_format(f, node);
 872                 q_data->width = f->fmt.pix.width;
 873                 q_data->height = f->fmt.pix.height;
 874                 q_data->height = f->fmt.pix.height;
 875
 876                 /* All parameters should have been set correctly by try_fmt */
 877                 q_data->bytesperline = f->fmt.pix.bytesperline;
 878                 q_data->sizeimage = f->fmt.pix.sizeimage;
 879
 880                 /* We must indicate which of the allowed colour spaces we have. */
 881                 q_data->colorspace = f->fmt.pix.colorspace;
 882         } else {
 883                 v4l2_dbg(1, debug, &dev->v4l2_dev,
 884                          "%s: Setting meta format for fmt: %08x, size %u\n",
 885                          __func__, f->fmt.meta.dataformat,
 886                          f->fmt.meta.buffersize);
 887
 888                 q_data->fmt = find_format(f, node);
 889                 q_data->width = 0;
 890                 q_data->height = 0;
 891                 q_data->bytesperline = 0;
 892                 q_data->sizeimage = f->fmt.meta.buffersize;
 893
 894                 /* This won't mean anything for metadata, but may as well fill it in. */
 895                 q_data->colorspace = V4L2_COLORSPACE_DEFAULT;
 896         }
 897
 898         v4l2_dbg(1, debug, &dev->v4l2_dev,
 899                  "%s: Calculated bpl as %u, size %u\n", __func__,
 900                  q_data->bytesperline, q_data->sizeimage);
 901
 902         setup_mmal_port_format(node, node->port);
 903         ret = vchiq_mmal_port_set_format(dev->mmal_instance, node->port);
 904         if (ret) {
 905                 v4l2_err(&dev->v4l2_dev,
 906                          "%s: Failed vchiq_mmal_port_set_format on port, ret %d\n",
 907                          __func__, ret);
 908                 ret = -EINVAL;
 909         }
 910
 911         if (q_data->sizeimage < node->port->minimum_buffer.size) {
 912                 v4l2_err(&dev->v4l2_dev,
 913                          "%s: Current buffer size of %u < min buf size %u - driver mismatch to MMAL\n",
 914                          __func__,
 915                          q_data->sizeimage,
 916                          node->port->minimum_buffer.size);
 917         }
 918
 919         v4l2_dbg(1, debug, &dev->v4l2_dev,
 920                  "%s: Set format for type %d, wxh: %dx%d, fmt: %08x, size %u\n",
 921                  __func__, f->type, q_data->width, q_data->height,
 922                  q_data->fmt->fourcc, q_data->sizeimage);
 923
 924         return ret;
 925 }
 926
 927 static int bcm2835_isp_node_querycap(struct file *file, void *priv,
 928                                      struct v4l2_capability *cap)
 929 {
 930         strscpy(cap->driver, BCM2835_ISP_NAME, sizeof(cap->driver));
 931         strscpy(cap->card, BCM2835_ISP_NAME, sizeof(cap->card));
 932         snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
 933                  BCM2835_ISP_NAME);
 934
 935         return 0;
 936 }
 937
 938 static int bcm2835_isp_node_g_fmt(struct file *file, void *priv,
 939                                   struct v4l2_format *f)
 940 {
 941         struct bcm2835_isp_node *node = video_drvdata(file);
 942
 943         if (f->type != node->queue.type)
 944                 return -EINVAL;
 945
 946         if (node_is_stats(node)) {
 947                 f->fmt.meta.dataformat = V4L2_META_FMT_BCM2835_ISP_STATS;
 948                 f->fmt.meta.buffersize =
 949                         node->port->minimum_buffer.size;
 950         } else {
 951                 struct bcm2835_isp_q_data *q_data = &node->q_data;
 952
 953                 f->fmt.pix.width = q_data->width;
 954                 f->fmt.pix.height = q_data->height;
 955                 f->fmt.pix.field = V4L2_FIELD_NONE;
 956                 f->fmt.pix.pixelformat = q_data->fmt->fourcc;
 957                 f->fmt.pix.bytesperline = q_data->bytesperline;
 958                 f->fmt.pix.sizeimage = q_data->sizeimage;
 959                 f->fmt.pix.colorspace = q_data->colorspace;
 960         }
 961
 962         return 0;
 963 }
 964
 965 static int bcm2835_isp_node_enum_fmt(struct file *file, void  *priv,
 966                                      struct v4l2_fmtdesc *f)
 967 {
 968         struct bcm2835_isp_node *node = video_drvdata(file);
 969
 970         if (f->type != node->queue.type)
 971                 return -EINVAL;
 972
 973         if (f->index < node->num_supported_fmts) {
 974                 /* Format found */
 975                 f->pixelformat = node->supported_fmts[f->index]->fourcc;
 976                 f->flags = 0;
 977                 return 0;
 978         }
 979
 980         return -EINVAL;
 981 }
 982
 983 static int bcm2835_isp_enum_framesizes(struct file *file, void *priv,
 984                                        struct v4l2_frmsizeenum *fsize)
 985 {
 986         struct bcm2835_isp_node *node = video_drvdata(file);
 987         struct bcm2835_isp_dev *dev = node_get_dev(node);
 988         const struct bcm2835_isp_fmt *fmt;
 989
 990         if (node_is_stats(node) || fsize->index)
 991                 return -EINVAL;
 992
 993         fmt = find_format_by_fourcc(fsize->pixel_format, node);
 994         if (!fmt) {
 995                 v4l2_err(&dev->v4l2_dev, "Invalid pixel code: %x\n",
 996                          fsize->pixel_format);
 997                 return -EINVAL;
 998         }
 999
1000         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
1001         fsize->stepwise.min_width = MIN_DIM;
1002         fsize->stepwise.max_width = MAX_DIM;
1003         fsize->stepwise.step_width = fmt->step_size;
1004
1005         fsize->stepwise.min_height = MIN_DIM;
1006         fsize->stepwise.max_height = MAX_DIM;
1007         fsize->stepwise.step_height = fmt->step_size;
1008
1009         return 0;
1010 }
1011
1012 static int bcm2835_isp_node_try_fmt(struct file *file, void *priv,
1013                                     struct v4l2_format *f)
1014 {
1015         struct bcm2835_isp_node *node = video_drvdata(file);
1016         const struct bcm2835_isp_fmt *fmt;
1017
1018         if (f->type != node->queue.type)
1019                 return -EINVAL;
1020
1021         fmt = find_format(f, node);
1022         if (!fmt)
1023                 fmt = get_default_format(node);
1024
1025         if (!node_is_stats(node)) {
1026                 int is_rgb;
1027
1028                 f->fmt.pix.width = max(min(f->fmt.pix.width, MAX_DIM),
1029                                        MIN_DIM);
1030                 f->fmt.pix.height = max(min(f->fmt.pix.height, MAX_DIM),
1031                                         MIN_DIM);
1032
1033                 f->fmt.pix.pixelformat = fmt->fourcc;
1034
1035                 /*
1036                  * Fill in the actual colour space when the requested one was
1037                  * not supported. This also catches the case when the "default"
1038                  * colour space was requested (as that's never in the mask).
1039                  */
1040                 if (!(V4L2_COLORSPACE_MASK(f->fmt.pix.colorspace) & fmt->colorspace_mask))
1041                         f->fmt.pix.colorspace = fmt->colorspace_default;
1042                 /* In all cases, we only support the defaults for these: */
1043                 f->fmt.pix.ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(f->fmt.pix.colorspace);
1044                 f->fmt.pix.xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(f->fmt.pix.colorspace);
1045                 /* RAW counts as sRGB here so that we get full range. */
1046                 is_rgb = f->fmt.pix.colorspace == V4L2_COLORSPACE_SRGB ||
1047                         f->fmt.pix.colorspace == V4L2_COLORSPACE_RAW;
1048                 f->fmt.pix.quantization = V4L2_MAP_QUANTIZATION_DEFAULT(is_rgb, f->fmt.pix.colorspace,
1049                                                                         f->fmt.pix.ycbcr_enc);
1050
1051                 f->fmt.pix.bytesperline = get_bytesperline(f->fmt.pix.width,
1052                                                            fmt);
1053                 f->fmt.pix.field = V4L2_FIELD_NONE;
1054                 f->fmt.pix.sizeimage =
1055                         get_sizeimage(f->fmt.pix.bytesperline, f->fmt.pix.width,
1056                                       f->fmt.pix.height, fmt);
1057         } else {
1058                 f->fmt.meta.dataformat = fmt->fourcc;
1059                 f->fmt.meta.buffersize = node->port->minimum_buffer.size;
1060         }
1061
1062         return 0;
1063 }
1064
1065 static int bcm2835_isp_node_s_fmt(struct file *file, void *priv,
1066                                   struct v4l2_format *f)
1067 {
1068         struct bcm2835_isp_node *node = video_drvdata(file);
1069         int ret;
1070
1071         if (f->type != node->queue.type)
1072                 return -EINVAL;
1073
1074         ret = bcm2835_isp_node_try_fmt(file, priv, f);
1075         if (ret)
1076                 return ret;
1077
1078         v4l2_dbg(1, debug, &node_get_dev(node)->v4l2_dev,
1079                  "%s: Set format for node %s[%d]\n",
1080                  __func__, node->name, node->id);
1081
1082         return populate_qdata_fmt(f, node);
1083 }
1084
1085 static int bcm2835_isp_node_s_selection(struct file *file, void *fh,
1086                                         struct v4l2_selection *s)
1087 {
1088         struct mmal_parameter_crop crop;
1089         struct bcm2835_isp_node *node = video_drvdata(file);
1090         struct bcm2835_isp_dev *dev = node_get_dev(node);
1091
1092         /* This return value is required fro V4L2 compliance. */
1093         if (node_is_stats(node))
1094                 return -ENOTTY;
1095
1096         if (!s->r.width || !s->r.height)
1097                 return -EINVAL;
1098
1099         /* We can only set crop on the input. */
1100         switch (s->target) {
1101         case V4L2_SEL_TGT_CROP:
1102                 /*
1103                  * Adjust the crop window if it goes outside of the frame
1104                  * dimensions.
1105                  */
1106                 s->r.left = min((unsigned int)max(s->r.left, 0),
1107                                 node->q_data.width - MIN_DIM);
1108                 s->r.top = min((unsigned int)max(s->r.top, 0),
1109                                node->q_data.height - MIN_DIM);
1110                 s->r.width = max(min(s->r.width,
1111                                      node->q_data.width - s->r.left), MIN_DIM);
1112                 s->r.height = max(min(s->r.height,
1113                                       node->q_data.height - s->r.top), MIN_DIM);
1114                 break;
1115         case V4L2_SEL_TGT_CROP_DEFAULT:
1116                 /* Default (i.e. no) crop window. */
1117                 s->r.left = 0;
1118                 s->r.top = 0;
1119                 s->r.width = node->q_data.width;
1120                 s->r.height = node->q_data.height;
1121                 break;
1122         default:
1123                 return -EINVAL;
1124         }
1125
1126         crop.rect.x = s->r.left;
1127         crop.rect.y = s->r.top;
1128         crop.rect.width = s->r.width;
1129         crop.rect.height = s->r.height;
1130
1131         return vchiq_mmal_port_parameter_set(dev->mmal_instance, node->port,
1132                                              MMAL_PARAMETER_CROP,
1133                                              &crop, sizeof(crop));
1134 }
1135
1136 static int bcm2835_isp_node_g_selection(struct file *file, void *fh,
1137                                         struct v4l2_selection *s)
1138 {
1139         struct mmal_parameter_crop crop;
1140         struct bcm2835_isp_node *node = video_drvdata(file);
1141         struct bcm2835_isp_dev *dev = node_get_dev(node);
1142         u32 crop_size = sizeof(crop);
1143         int ret;
1144
1145         /* We can only return out an input crop. */
1146         switch (s->target) {
1147         case V4L2_SEL_TGT_CROP:
1148                 ret = vchiq_mmal_port_parameter_get(dev->mmal_instance,
1149                                                     node->port,
1150                                                     MMAL_PARAMETER_CROP,
1151                                                     &crop, &crop_size);
1152                 if (!ret) {
1153                         s->r.left = crop.rect.x;
1154                         s->r.top = crop.rect.y;
1155                         s->r.width = crop.rect.width;
1156                         s->r.height = crop.rect.height;
1157                 }
1158                 break;
1159         case V4L2_SEL_TGT_CROP_DEFAULT:
1160         case V4L2_SEL_TGT_CROP_BOUNDS:
1161                 /* Default (i.e. no) crop window. */
1162                 s->r.left = 0;
1163                 s->r.top = 0;
1164                 s->r.width = node->q_data.width;
1165                 s->r.height = node->q_data.height;
1166                 ret = 0;
1167                 break;
1168         default:
1169                 ret =  -EINVAL;
1170         }
1171
1172         return ret;
1173 }
1174
1175 static int bcm3285_isp_subscribe_event(struct v4l2_fh *fh,
1176                                        const struct v4l2_event_subscription *s)
1177 {
1178         switch (s->type) {
1179         /* Cannot change source parameters dynamically at runtime. */
1180         case V4L2_EVENT_SOURCE_CHANGE:
1181                 return -EINVAL;
1182         case V4L2_EVENT_CTRL:
1183                 return v4l2_ctrl_subscribe_event(fh, s);
1184         default:
1185                 return v4l2_event_subscribe(fh, s, 4, NULL);
1186         }
1187 }
1188
1189 static const struct v4l2_ioctl_ops bcm2835_isp_node_ioctl_ops = {
1190         .vidioc_querycap                = bcm2835_isp_node_querycap,
1191         .vidioc_g_fmt_vid_cap           = bcm2835_isp_node_g_fmt,
1192         .vidioc_g_fmt_vid_out           = bcm2835_isp_node_g_fmt,
1193         .vidioc_g_fmt_meta_cap          = bcm2835_isp_node_g_fmt,
1194         .vidioc_s_fmt_vid_cap           = bcm2835_isp_node_s_fmt,
1195         .vidioc_s_fmt_vid_out           = bcm2835_isp_node_s_fmt,
1196         .vidioc_s_fmt_meta_cap          = bcm2835_isp_node_s_fmt,
1197         .vidioc_try_fmt_vid_cap         = bcm2835_isp_node_try_fmt,
1198         .vidioc_try_fmt_vid_out         = bcm2835_isp_node_try_fmt,
1199         .vidioc_try_fmt_meta_cap        = bcm2835_isp_node_try_fmt,
1200         .vidioc_s_selection             = bcm2835_isp_node_s_selection,
1201         .vidioc_g_selection             = bcm2835_isp_node_g_selection,
1202
1203         .vidioc_enum_fmt_vid_cap        = bcm2835_isp_node_enum_fmt,
1204         .vidioc_enum_fmt_vid_out        = bcm2835_isp_node_enum_fmt,
1205         .vidioc_enum_fmt_meta_cap       = bcm2835_isp_node_enum_fmt,
1206         .vidioc_enum_framesizes         = bcm2835_isp_enum_framesizes,
1207
1208         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1209         .vidioc_querybuf                = vb2_ioctl_querybuf,
1210         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1211         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1212         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1213         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1214         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1215
1216         .vidioc_streamon                = vb2_ioctl_streamon,
1217         .vidioc_streamoff               = vb2_ioctl_streamoff,
1218
1219         .vidioc_subscribe_event         = bcm3285_isp_subscribe_event,
1220         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1221 };
1222
1223 /*
1224  * Size of the array to provide to the VPU when asking for the list of supported
1225  * formats.
1226  *
1227  * The ISP component currently advertises 62 input formats, so add a small
1228  * overhead on that. Should the component advertise more formats then the excess
1229  * will be dropped and a warning logged.
1230  */
1231 #define MAX_SUPPORTED_ENCODINGS 70
1232
1233 /* Populate node->supported_fmts with the formats supported by those ports. */
1234 static int bcm2835_isp_get_supported_fmts(struct bcm2835_isp_node *node)
1235 {
1236         struct bcm2835_isp_dev *dev = node_get_dev(node);
1237         struct bcm2835_isp_fmt const **list;
1238         unsigned int i, j, num_encodings;
1239         u32 fourccs[MAX_SUPPORTED_ENCODINGS];
1240         u32 param_size = sizeof(fourccs);
1241         int ret;
1242
1243         ret = vchiq_mmal_port_parameter_get(dev->mmal_instance, node->port,
1244                                             MMAL_PARAMETER_SUPPORTED_ENCODINGS,
1245                                             &fourccs, &param_size);
1246
1247         if (ret) {
1248                 if (ret == MMAL_MSG_STATUS_ENOSPC) {
1249                         v4l2_err(&dev->v4l2_dev,
1250                                  "%s: port has more encodings than we provided space for. Some are dropped (%zu vs %u).\n",
1251                                  __func__, param_size / sizeof(u32),
1252                                  MAX_SUPPORTED_ENCODINGS);
1253                         num_encodings = MAX_SUPPORTED_ENCODINGS;
1254                 } else {
1255                         v4l2_err(&dev->v4l2_dev, "%s: get_param ret %u.\n",
1256                                  __func__, ret);
1257                         return -EINVAL;
1258                 }
1259         } else {
1260                 num_encodings = param_size / sizeof(u32);
1261         }
1262
1263         /*
1264          * Assume at this stage that all encodings will be supported in V4L2.
1265          * Any that aren't supported will waste a very small amount of memory.
1266          */
1267         list = devm_kzalloc(dev->dev,
1268                             sizeof(struct bcm2835_isp_fmt *) * num_encodings,
1269                             GFP_KERNEL);
1270         if (!list)
1271                 return -ENOMEM;
1272         node->supported_fmts = list;
1273
1274         for (i = 0, j = 0; i < num_encodings; i++) {
1275                 const struct bcm2835_isp_fmt *fmt = get_fmt(fourccs[i]);
1276
1277                 if (fmt) {
1278                         list[j] = fmt;
1279                         j++;
1280                 }
1281         }
1282         node->num_supported_fmts = j;
1283
1284         return 0;
1285 }
1286
1287 /*
1288  * Register a device node /dev/video<N> to go along with one of the ISP's input
1289  * or output nodes.
1290  */
1291 static int register_node(struct bcm2835_isp_dev *dev,
1292                          unsigned int instance,
1293                          struct bcm2835_isp_node *node,
1294                          int index)
1295 {
1296         struct video_device *vfd;
1297         struct vb2_queue *queue;
1298         int ret;
1299
1300         mutex_init(&node->lock);
1301         mutex_init(&node->queue_lock);
1302
1303         node->dev = dev;
1304         vfd = &node->vfd;
1305         queue = &node->queue;
1306         queue->type = index_to_queue_type(index);
1307         /*
1308          * Setup the node type-specific params.
1309          *
1310          * Only the OUTPUT node can set controls and crop windows. However,
1311          * we must allow the s/g_selection ioctl on the stats node as v4l2
1312          * compliance expects it to return a -ENOTTY, and the framework
1313          * does not handle it if the ioctl is disabled.
1314          */
1315         switch (queue->type) {
1316         case V4L2_BUF_TYPE_VIDEO_OUTPUT:
1317                 vfd->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
1318                 node->id = index;
1319                 node->vfl_dir = VFL_DIR_TX;
1320                 node->name = "output";
1321                 node->port = &dev->component->input[node->id];
1322                 break;
1323         case V4L2_BUF_TYPE_VIDEO_CAPTURE:
1324                 vfd->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
1325                 /* First Capture node starts at id 0, etc. */
1326                 node->id = index - BCM2835_ISP_NUM_OUTPUTS;
1327                 node->vfl_dir = VFL_DIR_RX;
1328                 node->name = "capture";
1329                 node->port = &dev->component->output[node->id];
1330                 v4l2_disable_ioctl(&node->vfd, VIDIOC_S_CTRL);
1331                 v4l2_disable_ioctl(&node->vfd, VIDIOC_S_SELECTION);
1332                 v4l2_disable_ioctl(&node->vfd, VIDIOC_G_SELECTION);
1333                 break;
1334         case V4L2_BUF_TYPE_META_CAPTURE:
1335                 vfd->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING;
1336                 node->id = index - BCM2835_ISP_NUM_OUTPUTS;
1337                 node->vfl_dir = VFL_DIR_RX;
1338                 node->name = "stats";
1339                 node->port = &dev->component->output[node->id];
1340                 v4l2_disable_ioctl(&node->vfd, VIDIOC_S_CTRL);
1341                 v4l2_disable_ioctl(&node->vfd, VIDIOC_S_SELECTION);
1342                 v4l2_disable_ioctl(&node->vfd, VIDIOC_G_SELECTION);
1343                 break;
1344         }
1345
1346         /* We use the selection API instead of the old crop API. */
1347         v4l2_disable_ioctl(vfd, VIDIOC_CROPCAP);
1348         v4l2_disable_ioctl(vfd, VIDIOC_G_CROP);
1349         v4l2_disable_ioctl(vfd, VIDIOC_S_CROP);
1350
1351         ret = bcm2835_isp_get_supported_fmts(node);
1352         if (ret)
1353                 return ret;
1354
1355         /* Initialise the video node. */
1356         vfd->vfl_type   = VFL_TYPE_VIDEO;
1357         vfd->fops       = &bcm2835_isp_fops,
1358         vfd->ioctl_ops  = &bcm2835_isp_node_ioctl_ops,
1359         vfd->minor      = -1,
1360         vfd->release    = video_device_release_empty,
1361         vfd->queue      = &node->queue;
1362         vfd->lock       = &node->lock;
1363         vfd->v4l2_dev   = &dev->v4l2_dev;
1364         vfd->vfl_dir    = node->vfl_dir;
1365
1366         node->q_data.fmt = get_default_format(node);
1367         node->q_data.width = DEFAULT_DIM;
1368         node->q_data.height = DEFAULT_DIM;
1369         node->q_data.bytesperline =
1370                 get_bytesperline(DEFAULT_DIM, node->q_data.fmt);
1371         node->q_data.sizeimage = node_is_stats(node) ?
1372                                  node->port->recommended_buffer.size :
1373                                  get_sizeimage(node->q_data.bytesperline,
1374                                                node->q_data.width,
1375                                                node->q_data.height,
1376                                                node->q_data.fmt);
1377         node->q_data.colorspace = node->q_data.fmt->colorspace_default;
1378
1379         queue->io_modes = VB2_MMAP | VB2_DMABUF;
1380         queue->drv_priv = node;
1381         queue->ops = &bcm2835_isp_node_queue_ops;
1382         queue->mem_ops = &vb2_dma_contig_memops;
1383         queue->buf_struct_size = sizeof(struct bcm2835_isp_buffer);
1384         queue->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1385         queue->dev = dev->dev;
1386         queue->lock = &node->queue_lock;
1387
1388         ret = vb2_queue_init(queue);
1389         if (ret < 0) {
1390                 v4l2_info(&dev->v4l2_dev, "vb2_queue_init failed\n");
1391                 return ret;
1392         }
1393
1394         /* Set some controls and defaults, but only on the VIDEO_OUTPUT node. */
1395         if (node_is_output(node)) {
1396                 unsigned int i;
1397
1398                 /* Use this ctrl template to assign custom ISP ctrls. */
1399                 struct v4l2_ctrl_config ctrl_template = {
1400                         .ops            = &bcm2835_isp_ctrl_ops,
1401                         .type           = V4L2_CTRL_TYPE_U8,
1402                         .def            = 0,
1403                         .min            = 0x00,
1404                         .max            = 0xff,
1405                         .step           = 1,
1406                 };
1407
1408                 /* 3 standard controls, and an array of custom controls */
1409                 ret = v4l2_ctrl_handler_init(&dev->ctrl_handler,
1410                                              3 + ARRAY_SIZE(custom_ctrls));
1411                 if (ret) {
1412                         v4l2_err(&dev->v4l2_dev, "ctrl_handler init failed (%d)\n",
1413                                  ret);
1414                         goto queue_cleanup;
1415                 }
1416
1417                 dev->r_gain = 1000;
1418                 dev->b_gain = 1000;
1419
1420                 v4l2_ctrl_new_std(&dev->ctrl_handler,  &bcm2835_isp_ctrl_ops,
1421                                   V4L2_CID_RED_BALANCE, 1, 0xffff, 1,
1422                                   dev->r_gain);
1423
1424                 v4l2_ctrl_new_std(&dev->ctrl_handler, &bcm2835_isp_ctrl_ops,
1425                                   V4L2_CID_BLUE_BALANCE, 1, 0xffff, 1,
1426                                   dev->b_gain);
1427
1428                 v4l2_ctrl_new_std(&dev->ctrl_handler, &bcm2835_isp_ctrl_ops,
1429                                   V4L2_CID_DIGITAL_GAIN, 1, 0xffff, 1, 1000);
1430
1431                 for (i = 0; i < ARRAY_SIZE(custom_ctrls); i++) {
1432                         ctrl_template.name = custom_ctrls[i].name;
1433                         ctrl_template.id = custom_ctrls[i].id;
1434                         ctrl_template.dims[0] = custom_ctrls[i].size;
1435                         ctrl_template.flags = custom_ctrls[i].flags;
1436                         v4l2_ctrl_new_custom(&dev->ctrl_handler,
1437                                              &ctrl_template, NULL);
1438                 }
1439
1440                 node->vfd.ctrl_handler = &dev->ctrl_handler;
1441                 if (dev->ctrl_handler.error) {
1442                         ret = dev->ctrl_handler.error;
1443                         v4l2_err(&dev->v4l2_dev, "controls init failed (%d)\n",
1444                                  ret);
1445                         v4l2_ctrl_handler_free(&dev->ctrl_handler);
1446                         goto ctrl_cleanup;
1447                 }
1448         }
1449
1450         /* Define the device names */
1451         snprintf(vfd->name, sizeof(node->vfd.name), "%s-%s%d", BCM2835_ISP_NAME,
1452                  node->name, node->id);
1453
1454         ret = video_register_device(vfd, VFL_TYPE_VIDEO, video_nr[instance]);
1455         if (ret) {
1456                 v4l2_err(&dev->v4l2_dev,
1457                          "Failed to register video %s[%d] device node\n",
1458                          node->name, node->id);
1459                 goto ctrl_cleanup;
1460         }
1461
1462         node->registered = true;
1463         video_set_drvdata(vfd, node);
1464
1465         v4l2_info(&dev->v4l2_dev,
1466                   "Device node %s[%d] registered as /dev/video%d\n",
1467                   node->name, node->id, vfd->num);
1468
1469         return 0;
1470
1471 ctrl_cleanup:
1472         if (node_is_output(node))
1473                 v4l2_ctrl_handler_free(&dev->ctrl_handler);
1474 queue_cleanup:
1475         vb2_queue_release(&node->queue);
1476         return ret;
1477 }
1478
1479 /* Unregister one of the /dev/video<N> nodes associated with the ISP. */
1480 static void bcm2835_unregister_node(struct bcm2835_isp_node *node)
1481 {
1482         struct bcm2835_isp_dev *dev = node_get_dev(node);
1483
1484         v4l2_info(&dev->v4l2_dev,
1485                   "Unregistering node %s[%d] device node /dev/video%d\n",
1486                   node->name, node->id, node->vfd.num);
1487
1488         if (node->registered) {
1489                 video_unregister_device(&node->vfd);
1490                 if (node_is_output(node))
1491                         v4l2_ctrl_handler_free(&dev->ctrl_handler);
1492                 vb2_queue_release(&node->queue);
1493         }
1494
1495         /*
1496          * node->supported_fmts.list is free'd automatically
1497          * as a managed resource.
1498          */
1499         node->supported_fmts = NULL;
1500         node->num_supported_fmts = 0;
1501         node->vfd.ctrl_handler = NULL;
1502         node->registered = false;
1503 }
1504
1505 static void media_controller_unregister(struct bcm2835_isp_dev *dev)
1506 {
1507         unsigned int i;
1508
1509         v4l2_info(&dev->v4l2_dev, "Unregister from media controller\n");
1510
1511         if (dev->media_device_registered) {
1512                 media_device_unregister(&dev->mdev);
1513                 media_device_cleanup(&dev->mdev);
1514                 dev->media_device_registered = false;
1515         }
1516
1517         kfree(dev->entity.name);
1518         dev->entity.name = NULL;
1519
1520         if (dev->media_entity_registered) {
1521                 media_device_unregister_entity(&dev->entity);
1522                 dev->media_entity_registered = false;
1523         }
1524
1525         for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1526                 struct bcm2835_isp_node *node = &dev->node[i];
1527
1528                 if (node->media_node_registered) {
1529                         media_remove_intf_links(node->intf_link->intf);
1530                         media_entity_remove_links(&dev->node[i].vfd.entity);
1531                         media_devnode_remove(node->intf_devnode);
1532                         media_device_unregister_entity(&node->vfd.entity);
1533                         kfree(node->vfd.entity.name);
1534                 }
1535                 node->media_node_registered = false;
1536         }
1537
1538         dev->v4l2_dev.mdev = NULL;
1539 }
1540
1541 static int media_controller_register_node(struct bcm2835_isp_dev *dev, int num)
1542 {
1543         struct bcm2835_isp_node *node = &dev->node[num];
1544         struct media_entity *entity = &node->vfd.entity;
1545         int output = node_is_output(node);
1546         char *name;
1547         int ret;
1548
1549         v4l2_info(&dev->v4l2_dev,
1550                   "Register %s node %d with media controller\n",
1551                   output ? "output" : "capture", num);
1552         entity->obj_type = MEDIA_ENTITY_TYPE_VIDEO_DEVICE;
1553         entity->function = MEDIA_ENT_F_IO_V4L;
1554         entity->info.dev.major = VIDEO_MAJOR;
1555         entity->info.dev.minor = node->vfd.minor;
1556         name = kmalloc(BCM2835_ISP_ENTITY_NAME_LEN, GFP_KERNEL);
1557         if (!name) {
1558                 ret = -ENOMEM;
1559                 goto error_no_mem;
1560         }
1561         snprintf(name, BCM2835_ISP_ENTITY_NAME_LEN, "%s0-%s%d",
1562                  BCM2835_ISP_NAME, output ? "output" : "capture", num);
1563         entity->name = name;
1564         node->pad.flags = output ? MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
1565         ret = media_entity_pads_init(entity, 1, &node->pad);
1566         if (ret)
1567                 goto error_pads_init;
1568         ret = media_device_register_entity(&dev->mdev, entity);
1569         if (ret)
1570                 goto error_register_entity;
1571
1572         node->intf_devnode = media_devnode_create(&dev->mdev,
1573                                                   MEDIA_INTF_T_V4L_VIDEO, 0,
1574                                                   VIDEO_MAJOR, node->vfd.minor);
1575         if (!node->intf_devnode) {
1576                 ret = -ENOMEM;
1577                 goto error_devnode_create;
1578         }
1579
1580         node->intf_link = media_create_intf_link(entity,
1581                                                  &node->intf_devnode->intf,
1582                                                  MEDIA_LNK_FL_IMMUTABLE |
1583                                                  MEDIA_LNK_FL_ENABLED);
1584         if (!node->intf_link) {
1585                 ret = -ENOMEM;
1586                 goto error_create_intf_link;
1587         }
1588
1589         if (output)
1590                 ret = media_create_pad_link(entity, 0, &dev->entity, num,
1591                                             MEDIA_LNK_FL_IMMUTABLE |
1592                                                     MEDIA_LNK_FL_ENABLED);
1593         else
1594                 ret = media_create_pad_link(&dev->entity, num, entity, 0,
1595                                             MEDIA_LNK_FL_IMMUTABLE |
1596                                             MEDIA_LNK_FL_ENABLED);
1597         if (ret)
1598                 goto error_create_pad_link;
1599
1600         dev->node[num].media_node_registered = true;
1601         return 0;
1602
1603 error_create_pad_link:
1604         media_remove_intf_links(&node->intf_devnode->intf);
1605 error_create_intf_link:
1606         media_devnode_remove(node->intf_devnode);
1607 error_devnode_create:
1608         media_device_unregister_entity(&node->vfd.entity);
1609 error_register_entity:
1610 error_pads_init:
1611         kfree(entity->name);
1612         entity->name = NULL;
1613 error_no_mem:
1614         if (ret)
1615                 v4l2_info(&dev->v4l2_dev, "Error registering node\n");
1616
1617         return ret;
1618 }
1619
1620 static int media_controller_register(struct bcm2835_isp_dev *dev)
1621 {
1622         char *name;
1623         unsigned int i;
1624         int ret;
1625
1626         v4l2_dbg(2, debug, &dev->v4l2_dev, "Registering with media controller\n");
1627         dev->mdev.dev = dev->dev;
1628         strscpy(dev->mdev.model, "bcm2835-isp",
1629                 sizeof(dev->mdev.model));
1630         strscpy(dev->mdev.bus_info, "platform:bcm2835-isp",
1631                 sizeof(dev->mdev.bus_info));
1632         media_device_init(&dev->mdev);
1633         dev->v4l2_dev.mdev = &dev->mdev;
1634
1635         v4l2_dbg(2, debug, &dev->v4l2_dev, "Register entity for nodes\n");
1636
1637         name = kmalloc(BCM2835_ISP_ENTITY_NAME_LEN, GFP_KERNEL);
1638         if (!name) {
1639                 ret = -ENOMEM;
1640                 goto done;
1641         }
1642         snprintf(name, BCM2835_ISP_ENTITY_NAME_LEN, "bcm2835_isp0");
1643         dev->entity.name = name;
1644         dev->entity.obj_type = MEDIA_ENTITY_TYPE_BASE;
1645         dev->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
1646
1647         for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1648                 dev->pad[i].flags = node_is_output(&dev->node[i]) ?
1649                                         MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
1650         }
1651
1652         ret = media_entity_pads_init(&dev->entity, BCM2835_ISP_NUM_NODES,
1653                                      dev->pad);
1654         if (ret)
1655                 goto done;
1656
1657         ret = media_device_register_entity(&dev->mdev, &dev->entity);
1658         if (ret)
1659                 goto done;
1660
1661         dev->media_entity_registered = true;
1662         for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1663                 ret = media_controller_register_node(dev, i);
1664                 if (ret)
1665                         goto done;
1666         }
1667
1668         ret = media_device_register(&dev->mdev);
1669         if (!ret)
1670                 dev->media_device_registered = true;
1671 done:
1672         return ret;
1673 }
1674
1675 static void bcm2835_isp_remove_instance(struct bcm2835_isp_dev *dev)
1676 {
1677         unsigned int i;
1678
1679         media_controller_unregister(dev);
1680
1681         for (i = 0; i < BCM2835_ISP_NUM_NODES; i++)
1682                 bcm2835_unregister_node(&dev->node[i]);
1683
1684         v4l2_device_unregister(&dev->v4l2_dev);
1685
1686         if (dev->component)
1687                 vchiq_mmal_component_finalise(dev->mmal_instance,
1688                                               dev->component);
1689
1690         vchiq_mmal_finalise(dev->mmal_instance);
1691 }
1692
1693 static int bcm2835_isp_probe_instance(struct platform_device *pdev,
1694                                       struct bcm2835_isp_dev **dev_int,
1695                                       unsigned int instance)
1696 {
1697         struct bcm2835_isp_dev *dev;
1698         unsigned int i;
1699         int ret;
1700
1701         dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
1702         if (!dev)
1703                 return -ENOMEM;
1704
1705         *dev_int = dev;
1706         dev->dev = &pdev->dev;
1707
1708         ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1709         if (ret)
1710                 return ret;
1711
1712         ret = vchiq_mmal_init(&dev->mmal_instance);
1713         if (ret) {
1714                 v4l2_device_unregister(&dev->v4l2_dev);
1715                 return ret;
1716         }
1717
1718         ret = vchiq_mmal_component_init(dev->mmal_instance, "ril.isp",
1719                                         &dev->component);
1720         if (ret) {
1721                 v4l2_err(&dev->v4l2_dev,
1722                          "%s: failed to create ril.isp component\n", __func__);
1723                 return ret;
1724         }
1725
1726         if (dev->component->inputs < BCM2835_ISP_NUM_OUTPUTS ||
1727             dev->component->outputs < BCM2835_ISP_NUM_CAPTURES +
1728                                         BCM2835_ISP_NUM_METADATA) {
1729                 v4l2_err(&dev->v4l2_dev,
1730                          "%s: ril.isp returned %d i/p (%d expected), %d o/p (%d expected) ports\n",
1731                           __func__, dev->component->inputs,
1732                           BCM2835_ISP_NUM_OUTPUTS,
1733                           dev->component->outputs,
1734                           BCM2835_ISP_NUM_CAPTURES + BCM2835_ISP_NUM_METADATA);
1735                 return -EINVAL;
1736         }
1737
1738         atomic_set(&dev->num_streaming, 0);
1739
1740         for (i = 0; i < BCM2835_ISP_NUM_NODES; i++) {
1741                 struct bcm2835_isp_node *node = &dev->node[i];
1742
1743                 ret = register_node(dev, instance, node, i);
1744                 if (ret)
1745                         return ret;
1746         }
1747
1748         ret = media_controller_register(dev);
1749         if (ret)
1750                 return ret;
1751
1752         return 0;
1753 }
1754
1755 static int bcm2835_isp_remove(struct platform_device *pdev)
1756 {
1757         struct bcm2835_isp_dev **bcm2835_isp_instances;
1758         unsigned int i;
1759
1760         bcm2835_isp_instances = platform_get_drvdata(pdev);
1761         for (i = 0; i < BCM2835_ISP_NUM_INSTANCES; i++) {
1762                 if (bcm2835_isp_instances[i])
1763                         bcm2835_isp_remove_instance(bcm2835_isp_instances[i]);
1764         }
1765
1766         return 0;
1767 }
1768
1769 static int bcm2835_isp_probe(struct platform_device *pdev)
1770 {
1771         struct bcm2835_isp_dev **bcm2835_isp_instances;
1772         unsigned int i;
1773         int ret;
1774
1775         bcm2835_isp_instances = devm_kzalloc(&pdev->dev,
1776                                              sizeof(bcm2835_isp_instances) *
1777                                                       BCM2835_ISP_NUM_INSTANCES,
1778                                              GFP_KERNEL);
1779         if (!bcm2835_isp_instances)
1780                 return -ENOMEM;
1781
1782         platform_set_drvdata(pdev, bcm2835_isp_instances);
1783
1784         for (i = 0; i < BCM2835_ISP_NUM_INSTANCES; i++) {
1785                 ret = bcm2835_isp_probe_instance(pdev,
1786                                                  &bcm2835_isp_instances[i], i);
1787                 if (ret)
1788                         goto error;
1789         }
1790
1791         dev_info(&pdev->dev, "Loaded V4L2 %s\n", BCM2835_ISP_NAME);
1792         return 0;
1793
1794 error:
1795         bcm2835_isp_remove(pdev);
1796
1797         return ret;
1798 }
1799
1800 static struct platform_driver bcm2835_isp_pdrv = {
1801         .probe = bcm2835_isp_probe,
1802         .remove = bcm2835_isp_remove,
1803         .driver = {
1804                         .name = BCM2835_ISP_NAME,
1805                   },
1806 };
1807
1808 module_platform_driver(bcm2835_isp_pdrv);
1809
1810 MODULE_DESCRIPTION("BCM2835 ISP driver");
1811 MODULE_AUTHOR("Naushir Patuck <naush@raspberrypi.com>");
1812 MODULE_LICENSE("GPL");
1813 MODULE_VERSION("1.0");
1814 MODULE_ALIAS("platform:bcm2835-isp");