target/linux/bcm27xx/patches-5.4/950-0652-media-bcm2835-unicam-Add-support-for-mulitple-device.patch

   1 From b466d74b45466b417e364c85c7fce71e9fc3fc7c Mon Sep 17 00:00:00 2001
   2 From: Naushir Patuck <naush@raspberrypi.com>
   3 Date: Tue, 7 Apr 2020 10:42:14 +0100
   4 Subject: [PATCH] media: bcm2835-unicam: Add support for mulitple
   5  device nodes.
   6
   7 Move device node specific state out of the device state structure and
   8 into a new node structure.  This separation will be needed for future
   9 changes where we will add an embedded data node to the driver to work
  10 alongside the existing image data node.
  11
  12 Currently only use a single image node, so this commit does not add
  13 any functional changes.
  14
  15 Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
  16 ---
  17  .../media/platform/bcm2835/bcm2835-unicam.c   | 484 ++++++++++--------
  18  1 file changed, 283 insertions(+), 201 deletions(-)
  19
  20 --- a/drivers/media/platform/bcm2835/bcm2835-unicam.c
  21 +++ b/drivers/media/platform/bcm2835/bcm2835-unicam.c
  22 @@ -109,7 +109,8 @@ MODULE_PARM_DESC(debug, "Debug level 0-3
  23  /* Define a nominal minimum image size */
  24  #define MIN_WIDTH      16
  25  #define MIN_HEIGHT     16
  26 -
  27 +/* Maximum number of simulataneous streams Uncaim can handle. */
  28 +#define MAX_NODES      2
  29  /*
  30   * struct unicam_fmt - Unicam media bus format information
  31   * @pixelformat: V4L2 pixel format FCC identifier. 0 if n/a.
  32 @@ -346,11 +347,37 @@ struct unicam_cfg {
  33
  34  #define MAX_POSSIBLE_PIX_FMTS (ARRAY_SIZE(formats))
  35
  36 -struct unicam_device {
  37 -       /* V4l2 specific parameters */
  38 +struct unicam_node {
  39 +       bool registered;
  40 +       unsigned int pad_id;
  41 +       /* Pointer pointing to current v4l2_buffer */
  42 +       struct unicam_buffer *cur_frm;
  43 +       /* Pointer pointing to next v4l2_buffer */
  44 +       struct unicam_buffer *next_frm;
  45 +       /* video capture */
  46 +       const struct unicam_fmt *fmt;
  47 +       /* Used to store current pixel format */
  48 +       struct v4l2_format v_fmt;
  49 +       /* Used to store current mbus frame format */
  50 +       struct v4l2_mbus_framefmt m_fmt;
  51 +       /* Buffer queue used in video-buf */
  52 +       struct vb2_queue buffer_queue;
  53 +       /* Queue of filled frames */
  54 +       struct unicam_dmaqueue dma_queue;
  55 +       /* IRQ lock for DMA queue */
  56 +       spinlock_t dma_queue_lock;
  57 +       /* lock used to access this structure */
  58 +       struct mutex lock;
  59         /* Identifies video device for this channel */
  60         struct video_device video_dev;
  61 +       /* Pointer to the parent handle */
  62 +       struct unicam_device *dev;
  63 +       struct media_pad pad;
  64         struct v4l2_ctrl_handler ctrl_handler;
  65 +};
  66 +
  67 +struct unicam_device {
  68 +       /* V4l2 specific parameters */
  69
  70         struct v4l2_fwnode_endpoint endpoint;
  71
  72 @@ -363,7 +390,6 @@ struct unicam_device {
  73         /* V4l2 device */
  74         struct v4l2_device v4l2_dev;
  75         struct media_device mdev;
  76 -       struct media_pad pad;
  77
  78         /* parent device */
  79         struct platform_device *pdev;
  80 @@ -378,18 +404,6 @@ struct unicam_device {
  81         /* current input at the sub device */
  82         int current_input;
  83
  84 -       /* Pointer pointing to current v4l2_buffer */
  85 -       struct unicam_buffer *cur_frm;
  86 -       /* Pointer pointing to next v4l2_buffer */
  87 -       struct unicam_buffer *next_frm;
  88 -
  89 -       /* video capture */
  90 -       const struct unicam_fmt *fmt;
  91 -       /* Used to store current pixel format */
  92 -       struct v4l2_format v_fmt;
  93 -       /* Used to store current mbus frame format */
  94 -       struct v4l2_mbus_framefmt m_fmt;
  95 -
  96         unsigned int virtual_channel;
  97         enum v4l2_mbus_type bus_type;
  98         /*
  99 @@ -401,20 +415,10 @@ struct unicam_device {
 100         unsigned int active_data_lanes;
 101
 102         struct v4l2_rect crop;
 103 -
 104 -       /* Currently selected input on subdev */
 105 -       int input;
 106 -
 107 -       /* Buffer queue used in video-buf */
 108 -       struct vb2_queue buffer_queue;
 109 -       /* Queue of filled frames */
 110 -       struct unicam_dmaqueue dma_queue;
 111 -       /* IRQ lock for DMA queue */
 112 -       spinlock_t dma_queue_lock;
 113 -       /* lock used to access this structure */
 114 -       struct mutex lock;
 115         /* Flag to denote that we are processing buffers */
 116         int streaming;
 117 +
 118 +       struct unicam_node node[MAX_NODES];
 119  };
 120
 121  /* Hardware access */
 122 @@ -526,10 +530,11 @@ static inline unsigned int bytes_per_lin
 123  }
 124
 125  static int __subdev_get_format(struct unicam_device *dev,
 126 -                              struct v4l2_mbus_framefmt *fmt)
 127 +                              struct v4l2_mbus_framefmt *fmt, int pad_id)
 128  {
 129         struct v4l2_subdev_format sd_fmt = {
 130                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 131 +               .pad = pad_id
 132         };
 133         int ret;
 134
 135 @@ -598,29 +603,30 @@ static int unicam_calc_format_size_bpl(s
 136         return 0;
 137  }
 138
 139 -static int unicam_reset_format(struct unicam_device *dev)
 140 +static int unicam_reset_format(struct unicam_node *node)
 141  {
 142 +       struct unicam_device *dev = node->dev;
 143         struct v4l2_mbus_framefmt mbus_fmt;
 144         int ret;
 145
 146 -       ret = __subdev_get_format(dev, &mbus_fmt);
 147 +       ret = __subdev_get_format(dev, &mbus_fmt, node->pad_id);
 148         if (ret) {
 149                 unicam_err(dev, "Failed to get_format - ret %d\n", ret);
 150                 return ret;
 151         }
 152
 153 -       if (mbus_fmt.code != dev->fmt->code) {
 154 +       if (mbus_fmt.code != dev->node[0].fmt->code) {
 155                 unicam_err(dev, "code mismatch - fmt->code %08x, mbus_fmt.code %08x\n",
 156 -                          dev->fmt->code, mbus_fmt.code);
 157 +                          dev->node[0].fmt->code, mbus_fmt.code);
 158                 return ret;
 159         }
 160
 161 -       v4l2_fill_pix_format(&dev->v_fmt.fmt.pix, &mbus_fmt);
 162 -       dev->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 163 +       v4l2_fill_pix_format(&dev->node[0].v_fmt.fmt.pix, &mbus_fmt);
 164 +       dev->node[0].v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 165
 166 -       unicam_calc_format_size_bpl(dev, dev->fmt, &dev->v_fmt);
 167 +       unicam_calc_format_size_bpl(dev, dev->node[0].fmt, &dev->node[0].v_fmt);
 168
 169 -       dev->m_fmt = mbus_fmt;
 170 +       dev->node[0].m_fmt = mbus_fmt;
 171
 172         return 0;
 173  }
 174 @@ -635,14 +641,14 @@ static void unicam_wr_dma_addr(struct un
 175
 176         reg_write(&dev->cfg, UNICAM_IBSA0, dmaaddr);
 177         reg_write(&dev->cfg, UNICAM_IBEA0,
 178 -                 dmaaddr + dev->v_fmt.fmt.pix.sizeimage);
 179 +                 dmaaddr + dev->node[0].v_fmt.fmt.pix.sizeimage);
 180  }
 181
 182  static inline unsigned int unicam_get_lines_done(struct unicam_device *dev)
 183  {
 184         dma_addr_t start_addr, cur_addr;
 185 -       unsigned int stride = dev->v_fmt.fmt.pix.bytesperline;
 186 -       struct unicam_buffer *frm = dev->cur_frm;
 187 +       unsigned int stride = dev->node[0].v_fmt.fmt.pix.bytesperline;
 188 +       struct unicam_buffer *frm = dev->node[0].cur_frm;
 189
 190         if (!frm)
 191                 return 0;
 192 @@ -654,12 +660,12 @@ static inline unsigned int unicam_get_li
 193
 194  static inline void unicam_schedule_next_buffer(struct unicam_device *dev)
 195  {
 196 -       struct unicam_dmaqueue *dma_q = &dev->dma_queue;
 197 +       struct unicam_dmaqueue *dma_q = &dev->node[0].dma_queue;
 198         struct unicam_buffer *buf;
 199         dma_addr_t addr;
 200
 201         buf = list_entry(dma_q->active.next, struct unicam_buffer, list);
 202 -       dev->next_frm = buf;
 203 +       dev->node[0].next_frm = buf;
 204         list_del(&buf->list);
 205
 206         addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
 207 @@ -668,11 +674,11 @@ static inline void unicam_schedule_next_
 208
 209  static inline void unicam_process_buffer_complete(struct unicam_device *dev)
 210  {
 211 -       dev->cur_frm->vb.field = dev->m_fmt.field;
 212 -       dev->cur_frm->vb.sequence = dev->sequence++;
 213 +       dev->node[0].cur_frm->vb.field = dev->node[0].m_fmt.field;
 214 +       dev->node[0].cur_frm->vb.sequence = dev->sequence++;
 215
 216 -       vb2_buffer_done(&dev->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
 217 -       dev->cur_frm = dev->next_frm;
 218 +       vb2_buffer_done(&dev->node[0].cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
 219 +       dev->node[0].cur_frm = dev->node[0].next_frm;
 220  }
 221
 222  /*
 223 @@ -687,7 +693,7 @@ static irqreturn_t unicam_isr(int irq, v
 224  {
 225         struct unicam_device *unicam = (struct unicam_device *)dev;
 226         struct unicam_cfg *cfg = &unicam->cfg;
 227 -       struct unicam_dmaqueue *dma_q = &unicam->dma_queue;
 228 +       struct unicam_dmaqueue *dma_q = &unicam->node[0].dma_queue;
 229         unsigned int lines_done = unicam_get_lines_done(dev);
 230         unsigned int sequence = unicam->sequence;
 231         int ista, sta;
 232 @@ -720,8 +726,9 @@ static irqreturn_t unicam_isr(int irq, v
 233                  * Timestamp is to be when the first data byte was captured,
 234                  * aka frame start.
 235                  */
 236 -               if (unicam->cur_frm)
 237 -                       unicam->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns();
 238 +               if (unicam->node[0].cur_frm)
 239 +                       unicam->node[0].cur_frm->vb.vb2_buf.timestamp =
 240 +                               ktime_get_ns();
 241         }
 242         if (ista & UNICAM_FEI || sta & UNICAM_PI0) {
 243                 /*
 244 @@ -729,7 +736,8 @@ static irqreturn_t unicam_isr(int irq, v
 245                  * stop the peripheral. Overwrite the frame we've just
 246                  * captured instead.
 247                  */
 248 -               if (unicam->cur_frm && unicam->cur_frm != unicam->next_frm)
 249 +               if (unicam->node[0].cur_frm &&
 250 +                   unicam->node[0].cur_frm != unicam->node[0].next_frm)
 251                         unicam_process_buffer_complete(unicam);
 252         }
 253
 254 @@ -738,11 +746,11 @@ static irqreturn_t unicam_isr(int irq, v
 255          * already started.
 256          */
 257         if (ista & (UNICAM_FSI | UNICAM_LCI) && !(ista & UNICAM_FEI)) {
 258 -               spin_lock(&unicam->dma_queue_lock);
 259 +               spin_lock(&unicam->node[0].dma_queue_lock);
 260                 if (!list_empty(&dma_q->active) &&
 261 -                   unicam->cur_frm == unicam->next_frm)
 262 +                   unicam->node[0].cur_frm == unicam->node[0].next_frm)
 263                         unicam_schedule_next_buffer(unicam);
 264 -               spin_unlock(&unicam->dma_queue_lock);
 265 +               spin_unlock(&unicam->node[0].dma_queue_lock);
 266         }
 267
 268         if (reg_read(&unicam->cfg, UNICAM_ICTL) & UNICAM_FCM) {
 269 @@ -756,7 +764,8 @@ static irqreturn_t unicam_isr(int irq, v
 270  static int unicam_querycap(struct file *file, void *priv,
 271                            struct v4l2_capability *cap)
 272  {
 273 -       struct unicam_device *dev = video_drvdata(file);
 274 +       struct unicam_node *node = video_drvdata(file);
 275 +       struct unicam_device *dev = node->dev;
 276
 277         strlcpy(cap->driver, UNICAM_MODULE_NAME, sizeof(cap->driver));
 278         strlcpy(cap->card, UNICAM_MODULE_NAME, sizeof(cap->card));
 279 @@ -770,7 +779,8 @@ static int unicam_querycap(struct file *
 280  static int unicam_enum_fmt_vid_cap(struct file *file, void  *priv,
 281                                    struct v4l2_fmtdesc *f)
 282  {
 283 -       struct unicam_device *dev = video_drvdata(file);
 284 +       struct unicam_node *node = video_drvdata(file);
 285 +       struct unicam_device *dev = node->dev;
 286         struct v4l2_subdev_mbus_code_enum mbus_code;
 287         const struct unicam_fmt *fmt = NULL;
 288         int index = 0;
 289 @@ -815,9 +825,9 @@ static int unicam_enum_fmt_vid_cap(struc
 290  static int unicam_g_fmt_vid_cap(struct file *file, void *priv,
 291                                 struct v4l2_format *f)
 292  {
 293 -       struct unicam_device *dev = video_drvdata(file);
 294 +       struct unicam_node *node = video_drvdata(file);
 295
 296 -       *f = dev->v_fmt;
 297 +       *f = node->v_fmt;
 298
 299         return 0;
 300  }
 301 @@ -859,9 +869,11 @@ const struct unicam_fmt *get_first_suppo
 302  static int unicam_try_fmt_vid_cap(struct file *file, void *priv,
 303                                   struct v4l2_format *f)
 304  {
 305 -       struct unicam_device *dev = video_drvdata(file);
 306 +       struct unicam_node *node = video_drvdata(file);
 307 +       struct unicam_device *dev = node->dev;
 308         struct v4l2_subdev_format sd_fmt = {
 309                 .which = V4L2_SUBDEV_FORMAT_TRY,
 310 +               .pad = 0
 311         };
 312         struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
 313         const struct unicam_fmt *fmt;
 314 @@ -939,8 +951,9 @@ static int unicam_try_fmt_vid_cap(struct
 315  static int unicam_s_fmt_vid_cap(struct file *file, void *priv,
 316                                 struct v4l2_format *f)
 317  {
 318 -       struct unicam_device *dev = video_drvdata(file);
 319 -       struct vb2_queue *q = &dev->buffer_queue;
 320 +       struct unicam_node *node = video_drvdata(file);
 321 +       struct unicam_device *dev = node->dev;
 322 +       struct vb2_queue *q = &node->buffer_queue;
 323         struct v4l2_mbus_framefmt mbus_fmt = {0};
 324         const struct unicam_fmt *fmt;
 325         int ret;
 326 @@ -985,17 +998,18 @@ static int unicam_s_fmt_vid_cap(struct f
 327                 return -EINVAL;
 328         }
 329
 330 -       dev->fmt = fmt;
 331 -       dev->v_fmt.fmt.pix.pixelformat = f->fmt.pix.pixelformat;
 332 -       dev->v_fmt.fmt.pix.bytesperline = f->fmt.pix.bytesperline;
 333 -       unicam_reset_format(dev);
 334 -
 335 -       unicam_dbg(3, dev, "%s %dx%d, mbus_fmt 0x%08X, V4L2 pix 0x%08X.\n",
 336 -                  __func__, dev->v_fmt.fmt.pix.width,
 337 -                  dev->v_fmt.fmt.pix.height, mbus_fmt.code,
 338 -                  dev->v_fmt.fmt.pix.pixelformat);
 339 +       node->fmt = fmt;
 340 +       node->v_fmt.fmt.pix.pixelformat = f->fmt.pix.pixelformat;
 341 +       node->v_fmt.fmt.pix.bytesperline = f->fmt.pix.bytesperline;
 342 +       unicam_reset_format(node);
 343 +
 344 +       unicam_dbg(3, dev,
 345 +                  "%s %dx%d, mbus_fmt 0x%08X, V4L2 pix 0x%08X.\n",
 346 +                  __func__, node->v_fmt.fmt.pix.width,
 347 +                  node->v_fmt.fmt.pix.height, mbus_fmt.code,
 348 +                  node->v_fmt.fmt.pix.pixelformat);
 349
 350 -       *f = dev->v_fmt;
 351 +       *f = node->v_fmt;
 352
 353         return 0;
 354  }
 355 @@ -1006,8 +1020,9 @@ static int unicam_queue_setup(struct vb2
 356                               unsigned int sizes[],
 357                               struct device *alloc_devs[])
 358  {
 359 -       struct unicam_device *dev = vb2_get_drv_priv(vq);
 360 -       unsigned int size = dev->v_fmt.fmt.pix.sizeimage;
 361 +       struct unicam_node *node = vb2_get_drv_priv(vq);
 362 +       struct unicam_device *dev = node->dev;
 363 +       unsigned int size = node->v_fmt.fmt.pix.sizeimage;
 364
 365         if (vq->num_buffers + *nbuffers < 3)
 366                 *nbuffers = 3 - vq->num_buffers;
 367 @@ -1029,15 +1044,16 @@ static int unicam_queue_setup(struct vb2
 368
 369  static int unicam_buffer_prepare(struct vb2_buffer *vb)
 370  {
 371 -       struct unicam_device *dev = vb2_get_drv_priv(vb->vb2_queue);
 372 +       struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue);
 373 +       struct unicam_device *dev = node->dev;
 374         struct unicam_buffer *buf = container_of(vb, struct unicam_buffer,
 375                                               vb.vb2_buf);
 376         unsigned long size;
 377
 378 -       if (WARN_ON(!dev->fmt))
 379 +       if (WARN_ON(!node->fmt))
 380                 return -EINVAL;
 381
 382 -       size = dev->v_fmt.fmt.pix.sizeimage;
 383 +       size = node->v_fmt.fmt.pix.sizeimage;
 384         if (vb2_plane_size(vb, 0) < size) {
 385                 unicam_err(dev, "data will not fit into plane (%lu < %lu)\n",
 386                            vb2_plane_size(vb, 0), size);
 387 @@ -1050,15 +1066,15 @@ static int unicam_buffer_prepare(struct
 388
 389  static void unicam_buffer_queue(struct vb2_buffer *vb)
 390  {
 391 -       struct unicam_device *dev = vb2_get_drv_priv(vb->vb2_queue);
 392 +       struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue);
 393         struct unicam_buffer *buf = container_of(vb, struct unicam_buffer,
 394                                               vb.vb2_buf);
 395 -       struct unicam_dmaqueue *dma_queue = &dev->dma_queue;
 396 +       struct unicam_dmaqueue *dma_queue = &node->dma_queue;
 397         unsigned long flags = 0;
 398
 399 -       spin_lock_irqsave(&dev->dma_queue_lock, flags);
 400 +       spin_lock_irqsave(&node->dma_queue_lock, flags);
 401         list_add_tail(&buf->list, &dma_queue->active);
 402 -       spin_unlock_irqrestore(&dev->dma_queue_lock, flags);
 403 +       spin_unlock_irqrestore(&node->dma_queue_lock, flags);
 404  }
 405
 406  static void unicam_set_packing_config(struct unicam_device *dev)
 407 @@ -1066,11 +1082,12 @@ static void unicam_set_packing_config(st
 408         int pack, unpack;
 409         u32 val;
 410
 411 -       if (dev->v_fmt.fmt.pix.pixelformat == dev->fmt->fourcc) {
 412 +       if (dev->node[0].v_fmt.fmt.pix.pixelformat ==
 413 +           dev->node[0].fmt->fourcc) {
 414                 unpack = UNICAM_PUM_NONE;
 415                 pack = UNICAM_PPM_NONE;
 416         } else {
 417 -               switch (dev->fmt->depth) {
 418 +               switch (dev->node[0].fmt->depth) {
 419                 case 8:
 420                         unpack = UNICAM_PUM_UNPACK8;
 421                         break;
 422 @@ -1108,17 +1125,17 @@ static void unicam_cfg_image_id(struct u
 423         if (dev->bus_type == V4L2_MBUS_CSI2_DPHY) {
 424                 /* CSI2 mode */
 425                 reg_write(cfg, UNICAM_IDI0,
 426 -                         (dev->virtual_channel << 6) | dev->fmt->csi_dt);
 427 +                       (dev->virtual_channel << 6) | dev->node[0].fmt->csi_dt);
 428         } else {
 429                 /* CCP2 mode */
 430 -               reg_write(cfg, UNICAM_IDI0, (0x80 | dev->fmt->csi_dt));
 431 +               reg_write(cfg, UNICAM_IDI0, (0x80 | dev->node[0].fmt->csi_dt));
 432         }
 433  }
 434
 435  static void unicam_start_rx(struct unicam_device *dev, unsigned long addr)
 436  {
 437         struct unicam_cfg *cfg = &dev->cfg;
 438 -       int line_int_freq = dev->v_fmt.fmt.pix.height >> 2;
 439 +       int line_int_freq = dev->node[0].v_fmt.fmt.pix.height >> 2;
 440         unsigned int i;
 441         u32 val;
 442
 443 @@ -1266,7 +1283,8 @@ static void unicam_start_rx(struct unica
 444                 reg_write(cfg, UNICAM_DAT3, val);
 445         }
 446
 447 -       reg_write(&dev->cfg, UNICAM_IBLS, dev->v_fmt.fmt.pix.bytesperline);
 448 +       reg_write(&dev->cfg, UNICAM_IBLS,
 449 +                 dev->node[0].v_fmt.fmt.pix.bytesperline);
 450         unicam_wr_dma_addr(dev, addr);
 451         unicam_set_packing_config(dev);
 452         unicam_cfg_image_id(dev);
 453 @@ -1327,21 +1345,22 @@ static void unicam_disable(struct unicam
 454
 455  static int unicam_start_streaming(struct vb2_queue *vq, unsigned int count)
 456  {
 457 -       struct unicam_device *dev = vb2_get_drv_priv(vq);
 458 -       struct unicam_dmaqueue *dma_q = &dev->dma_queue;
 459 +       struct unicam_node *node = vb2_get_drv_priv(vq);
 460 +       struct unicam_device *dev = node->dev;
 461 +       struct unicam_dmaqueue *dma_q = &node->dma_queue;
 462         struct unicam_buffer *buf, *tmp;
 463         unsigned long addr = 0;
 464         unsigned long flags;
 465         int ret;
 466
 467 -       spin_lock_irqsave(&dev->dma_queue_lock, flags);
 468 +       spin_lock_irqsave(&node->dma_queue_lock, flags);
 469         buf = list_entry(dma_q->active.next, struct unicam_buffer, list);
 470 -       dev->cur_frm = buf;
 471 -       dev->next_frm = buf;
 472 +       node->cur_frm = buf;
 473 +       node->next_frm = buf;
 474         list_del(&buf->list);
 475 -       spin_unlock_irqrestore(&dev->dma_queue_lock, flags);
 476 +       spin_unlock_irqrestore(&node->dma_queue_lock, flags);
 477
 478 -       addr = vb2_dma_contig_plane_dma_addr(&dev->cur_frm->vb.vb2_buf, 0);
 479 +       addr = vb2_dma_contig_plane_dma_addr(&node->cur_frm->vb.vb2_buf, 0);
 480         dev->sequence = 0;
 481
 482         ret = unicam_runtime_get(dev);
 483 @@ -1411,20 +1430,21 @@ err_release_buffers:
 484                 list_del(&buf->list);
 485                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 486         }
 487 -       if (dev->cur_frm != dev->next_frm)
 488 -               vb2_buffer_done(&dev->next_frm->vb.vb2_buf,
 489 +       if (node->cur_frm != node->next_frm)
 490 +               vb2_buffer_done(&node->next_frm->vb.vb2_buf,
 491                                 VB2_BUF_STATE_QUEUED);
 492 -       vb2_buffer_done(&dev->cur_frm->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 493 -       dev->next_frm = NULL;
 494 -       dev->cur_frm = NULL;
 495 +       vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 496 +       node->next_frm = NULL;
 497 +       node->cur_frm = NULL;
 498
 499         return ret;
 500  }
 501
 502  static void unicam_stop_streaming(struct vb2_queue *vq)
 503  {
 504 -       struct unicam_device *dev = vb2_get_drv_priv(vq);
 505 -       struct unicam_dmaqueue *dma_q = &dev->dma_queue;
 506 +       struct unicam_node *node = vb2_get_drv_priv(vq);
 507 +       struct unicam_device *dev = node->dev;
 508 +       struct unicam_dmaqueue *dma_q = &node->dma_queue;
 509         struct unicam_buffer *buf, *tmp;
 510         unsigned long flags;
 511
 512 @@ -1434,22 +1454,24 @@ static void unicam_stop_streaming(struct
 513         unicam_disable(dev);
 514
 515         /* Release all active buffers */
 516 -       spin_lock_irqsave(&dev->dma_queue_lock, flags);
 517 +       spin_lock_irqsave(&node->dma_queue_lock, flags);
 518         list_for_each_entry_safe(buf, tmp, &dma_q->active, list) {
 519                 list_del(&buf->list);
 520                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 521         }
 522
 523 -       if (dev->cur_frm == dev->next_frm) {
 524 -               vb2_buffer_done(&dev->cur_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 525 +       if (node->cur_frm == node->next_frm) {
 526 +               vb2_buffer_done(&node->cur_frm->vb.vb2_buf,
 527 +                               VB2_BUF_STATE_ERROR);
 528         } else {
 529 -               vb2_buffer_done(&dev->cur_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 530 -               vb2_buffer_done(&dev->next_frm->vb.vb2_buf,
 531 +               vb2_buffer_done(&node->cur_frm->vb.vb2_buf,
 532 +                               VB2_BUF_STATE_ERROR);
 533 +               vb2_buffer_done(&node->next_frm->vb.vb2_buf,
 534                                 VB2_BUF_STATE_ERROR);
 535         }
 536 -       dev->cur_frm = NULL;
 537 -       dev->next_frm = NULL;
 538 -       spin_unlock_irqrestore(&dev->dma_queue_lock, flags);
 539 +       node->cur_frm = NULL;
 540 +       node->next_frm = NULL;
 541 +       spin_unlock_irqrestore(&node->dma_queue_lock, flags);
 542
 543         clk_disable_unprepare(dev->clock);
 544         unicam_runtime_put(dev);
 545 @@ -1458,7 +1480,8 @@ static void unicam_stop_streaming(struct
 546  static int unicam_enum_input(struct file *file, void *priv,
 547                              struct v4l2_input *inp)
 548  {
 549 -       struct unicam_device *dev = video_drvdata(file);
 550 +       struct unicam_node *node = video_drvdata(file);
 551 +       struct unicam_device *dev = node->dev;
 552
 553         if (inp->index != 0)
 554                 return -EINVAL;
 555 @@ -1506,21 +1529,24 @@ static int unicam_s_input(struct file *f
 556  static int unicam_querystd(struct file *file, void *priv,
 557                            v4l2_std_id *std)
 558  {
 559 -       struct unicam_device *dev = video_drvdata(file);
 560 +       struct unicam_node *node = video_drvdata(file);
 561 +       struct unicam_device *dev = node->dev;
 562
 563         return v4l2_subdev_call(dev->sensor, video, querystd, std);
 564  }
 565
 566  static int unicam_g_std(struct file *file, void *priv, v4l2_std_id *std)
 567  {
 568 -       struct unicam_device *dev = video_drvdata(file);
 569 +       struct unicam_node *node = video_drvdata(file);
 570 +       struct unicam_device *dev = node->dev;
 571
 572         return v4l2_subdev_call(dev->sensor, video, g_std, std);
 573  }
 574
 575  static int unicam_s_std(struct file *file, void *priv, v4l2_std_id std)
 576  {
 577 -       struct unicam_device *dev = video_drvdata(file);
 578 +       struct unicam_node *node = video_drvdata(file);
 579 +       struct unicam_device *dev = node->dev;
 580         int ret;
 581         v4l2_std_id current_std;
 582
 583 @@ -1531,29 +1557,31 @@ static int unicam_s_std(struct file *fil
 584         if (std == current_std)
 585                 return 0;
 586
 587 -       if (vb2_is_busy(&dev->buffer_queue))
 588 +       if (vb2_is_busy(&node->buffer_queue))
 589                 return -EBUSY;
 590
 591         ret = v4l2_subdev_call(dev->sensor, video, s_std, std);
 592
 593         /* Force recomputation of bytesperline */
 594 -       dev->v_fmt.fmt.pix.bytesperline = 0;
 595 +       node->v_fmt.fmt.pix.bytesperline = 0;
 596
 597 -       unicam_reset_format(dev);
 598 +       unicam_reset_format(node);
 599
 600         return ret;
 601  }
 602
 603  static int unicam_s_edid(struct file *file, void *priv, struct v4l2_edid *edid)
 604  {
 605 -       struct unicam_device *dev = video_drvdata(file);
 606 +       struct unicam_node *node = video_drvdata(file);
 607 +       struct unicam_device *dev = node->dev;
 608
 609         return v4l2_subdev_call(dev->sensor, pad, set_edid, edid);
 610  }
 611
 612  static int unicam_g_edid(struct file *file, void *priv, struct v4l2_edid *edid)
 613  {
 614 -       struct unicam_device *dev = video_drvdata(file);
 615 +       struct unicam_node *node = video_drvdata(file);
 616 +       struct unicam_device *dev = node->dev;
 617
 618         return v4l2_subdev_call(dev->sensor, pad, get_edid, edid);
 619  }
 620 @@ -1561,7 +1589,8 @@ static int unicam_g_edid(struct file *fi
 621  static int unicam_enum_framesizes(struct file *file, void *priv,
 622                                   struct v4l2_frmsizeenum *fsize)
 623  {
 624 -       struct unicam_device *dev = video_drvdata(file);
 625 +       struct unicam_node *node = video_drvdata(file);
 626 +       struct unicam_device *dev = node->dev;
 627         const struct unicam_fmt *fmt;
 628         struct v4l2_subdev_frame_size_enum fse;
 629         int ret;
 630 @@ -1596,7 +1625,8 @@ static int unicam_enum_framesizes(struct
 631  static int unicam_enum_frameintervals(struct file *file, void *priv,
 632                                       struct v4l2_frmivalenum *fival)
 633  {
 634 -       struct unicam_device *dev = video_drvdata(file);
 635 +       struct unicam_node *node = video_drvdata(file);
 636 +       struct unicam_device *dev = node->dev;
 637         const struct unicam_fmt *fmt;
 638         struct v4l2_subdev_frame_interval_enum fie = {
 639                 .index = fival->index,
 640 @@ -1624,14 +1654,16 @@ static int unicam_enum_frameintervals(st
 641
 642  static int unicam_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
 643  {
 644 -       struct unicam_device *dev = video_drvdata(file);
 645 +       struct unicam_node *node = video_drvdata(file);
 646 +       struct unicam_device *dev = node->dev;
 647
 648         return v4l2_g_parm_cap(video_devdata(file), dev->sensor, a);
 649  }
 650
 651  static int unicam_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
 652  {
 653 -       struct unicam_device *dev = video_drvdata(file);
 654 +       struct unicam_node *node = video_drvdata(file);
 655 +       struct unicam_device *dev = node->dev;
 656
 657         return v4l2_s_parm_cap(video_devdata(file), dev->sensor, a);
 658  }
 659 @@ -1639,7 +1671,8 @@ static int unicam_s_parm(struct file *fi
 660  static int unicam_g_dv_timings(struct file *file, void *priv,
 661                                struct v4l2_dv_timings *timings)
 662  {
 663 -       struct unicam_device *dev = video_drvdata(file);
 664 +       struct unicam_node *node = video_drvdata(file);
 665 +       struct unicam_device *dev = node->dev;
 666
 667         return v4l2_subdev_call(dev->sensor, video, g_dv_timings, timings);
 668  }
 669 @@ -1647,7 +1680,8 @@ static int unicam_g_dv_timings(struct fi
 670  static int unicam_s_dv_timings(struct file *file, void *priv,
 671                                struct v4l2_dv_timings *timings)
 672  {
 673 -       struct unicam_device *dev = video_drvdata(file);
 674 +       struct unicam_node *node = video_drvdata(file);
 675 +       struct unicam_device *dev = node->dev;
 676         struct v4l2_dv_timings current_timings;
 677         int ret;
 678
 679 @@ -1657,15 +1691,15 @@ static int unicam_s_dv_timings(struct fi
 680         if (v4l2_match_dv_timings(timings, &current_timings, 0, false))
 681                 return 0;
 682
 683 -       if (vb2_is_busy(&dev->buffer_queue))
 684 +       if (vb2_is_busy(&node->buffer_queue))
 685                 return -EBUSY;
 686
 687         ret = v4l2_subdev_call(dev->sensor, video, s_dv_timings, timings);
 688
 689         /* Force recomputation of bytesperline */
 690 -       dev->v_fmt.fmt.pix.bytesperline = 0;
 691 +       node->v_fmt.fmt.pix.bytesperline = 0;
 692
 693 -       unicam_reset_format(dev);
 694 +       unicam_reset_format(node);
 695
 696         return ret;
 697  }
 698 @@ -1673,7 +1707,8 @@ static int unicam_s_dv_timings(struct fi
 699  static int unicam_query_dv_timings(struct file *file, void *priv,
 700                                    struct v4l2_dv_timings *timings)
 701  {
 702 -       struct unicam_device *dev = video_drvdata(file);
 703 +       struct unicam_node *node = video_drvdata(file);
 704 +       struct unicam_device *dev = node->dev;
 705
 706         return v4l2_subdev_call(dev->sensor, video, query_dv_timings, timings);
 707  }
 708 @@ -1681,7 +1716,8 @@ static int unicam_query_dv_timings(struc
 709  static int unicam_enum_dv_timings(struct file *file, void *priv,
 710                                   struct v4l2_enum_dv_timings *timings)
 711  {
 712 -       struct unicam_device *dev = video_drvdata(file);
 713 +       struct unicam_node *node = video_drvdata(file);
 714 +       struct unicam_device *dev = node->dev;
 715
 716         return v4l2_subdev_call(dev->sensor, pad, enum_dv_timings, timings);
 717  }
 718 @@ -1689,7 +1725,8 @@ static int unicam_enum_dv_timings(struct
 719  static int unicam_dv_timings_cap(struct file *file, void *priv,
 720                                  struct v4l2_dv_timings_cap *cap)
 721  {
 722 -       struct unicam_device *dev = video_drvdata(file);
 723 +       struct unicam_node *node = video_drvdata(file);
 724 +       struct unicam_device *dev = node->dev;
 725
 726         return v4l2_subdev_call(dev->sensor, pad, dv_timings_cap, cap);
 727  }
 728 @@ -1707,7 +1744,8 @@ static int unicam_subscribe_event(struct
 729
 730  static int unicam_log_status(struct file *file, void *fh)
 731  {
 732 -       struct unicam_device *dev = video_drvdata(file);
 733 +       struct unicam_node *node = video_drvdata(file);
 734 +       struct unicam_device *dev = node->dev;
 735         struct unicam_cfg *cfg = &dev->cfg;
 736         u32 reg;
 737
 738 @@ -1716,10 +1754,10 @@ static int unicam_log_status(struct file
 739
 740         unicam_info(dev, "-----Receiver status-----\n");
 741         unicam_info(dev, "V4L2 width/height:   %ux%u\n",
 742 -                   dev->v_fmt.fmt.pix.width, dev->v_fmt.fmt.pix.height);
 743 -       unicam_info(dev, "Mediabus format:     %08x\n", dev->fmt->code);
 744 +                   node->v_fmt.fmt.pix.width, node->v_fmt.fmt.pix.height);
 745 +       unicam_info(dev, "Mediabus format:     %08x\n", node->fmt->code);
 746         unicam_info(dev, "V4L2 format:         %08x\n",
 747 -                   dev->v_fmt.fmt.pix.pixelformat);
 748 +                   node->v_fmt.fmt.pix.pixelformat);
 749         reg = reg_read(&dev->cfg, UNICAM_IPIPE);
 750         unicam_info(dev, "Unpacking/packing:   %u / %u\n",
 751                     get_field(reg, UNICAM_PUM_MASK),
 752 @@ -1744,7 +1782,7 @@ static void unicam_notify(struct v4l2_su
 753
 754         switch (notification) {
 755         case V4L2_DEVICE_NOTIFY_EVENT:
 756 -               v4l2_event_queue(&dev->video_dev, arg);
 757 +               v4l2_event_queue(&dev->node[0].video_dev, arg);
 758                 break;
 759         default:
 760                 break;
 761 @@ -1767,10 +1805,11 @@ static const struct vb2_ops unicam_video
 762   */
 763  static int unicam_open(struct file *file)
 764  {
 765 -       struct unicam_device *dev = video_drvdata(file);
 766 +       struct unicam_node *node = video_drvdata(file);
 767 +       struct unicam_device *dev = node->dev;
 768         int ret;
 769
 770 -       mutex_lock(&dev->lock);
 771 +       mutex_lock(&node->lock);
 772
 773         ret = v4l2_fh_open(file);
 774         if (ret) {
 775 @@ -1790,18 +1829,19 @@ static int unicam_open(struct file *file
 776         ret = 0;
 777
 778  unlock:
 779 -       mutex_unlock(&dev->lock);
 780 +       mutex_unlock(&node->lock);
 781         return ret;
 782  }
 783
 784  static int unicam_release(struct file *file)
 785  {
 786 -       struct unicam_device *dev = video_drvdata(file);
 787 +       struct unicam_node *node = video_drvdata(file);
 788 +       struct unicam_device *dev = node->dev;
 789         struct v4l2_subdev *sd = dev->sensor;
 790         bool fh_singular;
 791         int ret;
 792
 793 -       mutex_lock(&dev->lock);
 794 +       mutex_lock(&node->lock);
 795
 796         fh_singular = v4l2_fh_is_singular_file(file);
 797
 798 @@ -1810,7 +1850,7 @@ static int unicam_release(struct file *f
 799         if (fh_singular)
 800                 v4l2_subdev_call(sd, core, s_power, 0);
 801
 802 -       mutex_unlock(&dev->lock);
 803 +       mutex_unlock(&node->lock);
 804
 805         return ret;
 806  }
 807 @@ -1892,7 +1932,8 @@ unicam_async_bound(struct v4l2_async_not
 808         return 0;
 809  }
 810
 811 -static int unicam_probe_complete(struct unicam_device *unicam)
 812 +static int register_node(struct unicam_device *unicam, struct unicam_node *node,
 813 +                        enum v4l2_buf_type type, int pad_id)
 814  {
 815         struct video_device *vdev;
 816         struct vb2_queue *q;
 817 @@ -1900,15 +1941,7 @@ static int unicam_probe_complete(struct
 818         const struct unicam_fmt *fmt;
 819         int ret;
 820
 821 -       v4l2_set_subdev_hostdata(unicam->sensor, unicam);
 822 -
 823 -       unicam->v4l2_dev.notify = unicam_notify;
 824 -
 825 -       unicam->sensor_config = v4l2_subdev_alloc_pad_config(unicam->sensor);
 826 -       if (!unicam->sensor_config)
 827 -               return -ENOMEM;
 828 -
 829 -       ret = __subdev_get_format(unicam, &mbus_fmt);
 830 +       ret = __subdev_get_format(unicam, &mbus_fmt, pad_id);
 831         if (ret) {
 832                 unicam_err(unicam, "Failed to get_format - ret %d\n", ret);
 833                 return ret;
 834 @@ -1938,14 +1971,15 @@ static int unicam_probe_complete(struct
 835                         return -EINVAL;
 836         }
 837
 838 -       unicam->fmt = fmt;
 839 +       node->pad_id = pad_id;
 840 +       node->fmt = fmt;
 841         if (fmt->fourcc)
 842 -               unicam->v_fmt.fmt.pix.pixelformat = fmt->fourcc;
 843 +               node->v_fmt.fmt.pix.pixelformat = fmt->fourcc;
 844         else
 845 -               unicam->v_fmt.fmt.pix.pixelformat = fmt->repacked_fourcc;
 846 +               node->v_fmt.fmt.pix.pixelformat = fmt->repacked_fourcc;
 847
 848         /* Read current subdev format */
 849 -       unicam_reset_format(unicam);
 850 +       unicam_reset_format(node);
 851
 852         if (v4l2_subdev_has_op(unicam->sensor, video, s_std)) {
 853                 v4l2_std_id tvnorms;
 854 @@ -1962,27 +1996,30 @@ static int unicam_probe_complete(struct
 855                                        g_tvnorms, &tvnorms);
 856                 if (WARN_ON(ret))
 857                         return -EINVAL;
 858 -               unicam->video_dev.tvnorms |= tvnorms;
 859 +               node->video_dev.tvnorms |= tvnorms;
 860         }
 861
 862 -       spin_lock_init(&unicam->dma_queue_lock);
 863 -       mutex_init(&unicam->lock);
 864 +       spin_lock_init(&node->dma_queue_lock);
 865 +       mutex_init(&node->lock);
 866
 867 -       /* Add controls from the subdevice */
 868 -       ret = v4l2_ctrl_add_handler(&unicam->ctrl_handler,
 869 -                                   unicam->sensor->ctrl_handler, NULL, true);
 870 -       if (ret < 0)
 871 -               return ret;
 872 +       if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 873 +               /* Add controls from the subdevice */
 874 +               ret = v4l2_ctrl_add_handler(&node->ctrl_handler,
 875 +                                           unicam->sensor->ctrl_handler, NULL,
 876 +                                           true);
 877 +               if (ret < 0)
 878 +                       return ret;
 879 +       }
 880
 881 -       q = &unicam->buffer_queue;
 882 -       q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 883 +       q = &node->buffer_queue;
 884 +       q->type = type;
 885         q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ;
 886 -       q->drv_priv = unicam;
 887 +       q->drv_priv = node;
 888         q->ops = &unicam_video_qops;
 889         q->mem_ops = &vb2_dma_contig_memops;
 890         q->buf_struct_size = sizeof(struct unicam_buffer);
 891         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 892 -       q->lock = &unicam->lock;
 893 +       q->lock = &node->lock;
 894         q->min_buffers_needed = 2;
 895         q->dev = &unicam->pdev->dev;
 896
 897 @@ -1992,9 +2029,9 @@ static int unicam_probe_complete(struct
 898                 return ret;
 899         }
 900
 901 -       INIT_LIST_HEAD(&unicam->dma_queue.active);
 902 +       INIT_LIST_HEAD(&node->dma_queue.active);
 903
 904 -       vdev = &unicam->video_dev;
 905 +       vdev = &node->video_dev;
 906         strlcpy(vdev->name, UNICAM_MODULE_NAME, sizeof(vdev->name));
 907         vdev->release = video_device_release_empty;
 908         vdev->fops = &unicam_fops;
 909 @@ -2002,69 +2039,113 @@ static int unicam_probe_complete(struct
 910         vdev->v4l2_dev = &unicam->v4l2_dev;
 911         vdev->vfl_dir = VFL_DIR_RX;
 912         vdev->queue = q;
 913 -       vdev->lock = &unicam->lock;
 914 +       vdev->lock = &node->lock;
 915         vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
 916                             V4L2_CAP_READWRITE;
 917 -
 918         /* If the source has no controls then remove our ctrl handler. */
 919 -       if (list_empty(&unicam->ctrl_handler.ctrls))
 920 +       if (list_empty(&node->ctrl_handler.ctrls))
 921                 unicam->v4l2_dev.ctrl_handler = NULL;
 922
 923 -       video_set_drvdata(vdev, unicam);
 924 +       node->dev = unicam;
 925 +       video_set_drvdata(vdev, node);
 926         vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT;
 927
 928         if (!v4l2_subdev_has_op(unicam->sensor, video, s_std)) {
 929 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_S_STD);
 930 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_G_STD);
 931 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_ENUMSTD);
 932 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_STD);
 933 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_STD);
 934 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUMSTD);
 935         }
 936         if (!v4l2_subdev_has_op(unicam->sensor, video, querystd))
 937 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_QUERYSTD);
 938 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_QUERYSTD);
 939         if (!v4l2_subdev_has_op(unicam->sensor, video, s_dv_timings)) {
 940 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_S_EDID);
 941 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_G_EDID);
 942 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_DV_TIMINGS_CAP);
 943 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_G_DV_TIMINGS);
 944 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_S_DV_TIMINGS);
 945 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_ENUM_DV_TIMINGS);
 946 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_QUERY_DV_TIMINGS);
 947 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_EDID);
 948 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_EDID);
 949 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_DV_TIMINGS_CAP);
 950 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_DV_TIMINGS);
 951 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_DV_TIMINGS);
 952 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_DV_TIMINGS);
 953 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_QUERY_DV_TIMINGS);
 954         }
 955         if (!v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_interval))
 956 -               v4l2_disable_ioctl(&unicam->video_dev,
 957 +               v4l2_disable_ioctl(&node->video_dev,
 958                                    VIDIOC_ENUM_FRAMEINTERVALS);
 959         if (!v4l2_subdev_has_op(unicam->sensor, video, g_frame_interval))
 960 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_G_PARM);
 961 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_PARM);
 962         if (!v4l2_subdev_has_op(unicam->sensor, video, s_frame_interval))
 963 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_S_PARM);
 964 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_PARM);
 965
 966         if (!v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_size))
 967 -               v4l2_disable_ioctl(&unicam->video_dev, VIDIOC_ENUM_FRAMESIZES);
 968 +               v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_FRAMESIZES);
 969
 970         ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
 971         if (ret) {
 972                 unicam_err(unicam, "Unable to register video device.\n");
 973                 return ret;
 974         }
 975 +       node->registered = true;
 976
 977 -       ret = v4l2_device_register_ro_subdev_nodes(&unicam->v4l2_dev);
 978 +       ret = media_create_pad_link(&unicam->sensor->entity,
 979 +                                   0, &node->video_dev.entity, 0,
 980 +                                   MEDIA_LNK_FL_ENABLED |
 981 +                                   MEDIA_LNK_FL_IMMUTABLE);
 982 +       if (ret)
 983 +               unicam_err(unicam, "Unable to create pad links.\n");
 984 +
 985 +       return ret;
 986 +}
 987 +
 988 +static void unregister_nodes(struct unicam_device *unicam)
 989 +{
 990 +       if (unicam->node[0].registered) {
 991 +               video_unregister_device(&unicam->node[0].video_dev);
 992 +               unicam->node[0].registered = false;
 993 +       }
 994 +       if (unicam->node[1].registered) {
 995 +               video_unregister_device(&unicam->node[1].video_dev);
 996 +               unicam->node[1].registered = false;
 997 +       }
 998 +}
 999 +
1000 +static int unicam_probe_complete(struct unicam_device *unicam)
1001 +{
1002 +       int ret;
1003 +
1004 +       v4l2_set_subdev_hostdata(unicam->sensor, unicam);
1005 +
1006 +       unicam->v4l2_dev.notify = unicam_notify;
1007 +
1008 +       unicam->sensor_config = v4l2_subdev_alloc_pad_config(unicam->sensor);
1009 +       if (!unicam->sensor_config)
1010 +               return -ENOMEM;
1011 +
1012 +       ret = register_node(unicam, &unicam->node[0],
1013 +                           V4L2_BUF_TYPE_VIDEO_CAPTURE, 0);
1014         if (ret) {
1015 -               unicam_err(unicam,
1016 -                          "Unable to register subdev nodes.\n");
1017 -               video_unregister_device(&unicam->video_dev);
1018 -               return ret;
1019 +               unicam_err(unicam, "Unable to register subdev node 0.\n");
1020 +               goto unregister;
1021 +       }
1022 +       if (unicam->sensor->entity.num_pads >= 2) {
1023 +               ret = register_node(unicam, &unicam->node[1],
1024 +                                   V4L2_BUF_TYPE_META_CAPTURE, 1);
1025 +               if (ret) {
1026 +                       unicam_err(unicam,
1027 +                                  "Unable to register subdev node 1.\n");
1028 +                       goto unregister;
1029 +               }
1030         }
1031
1032 -       ret = media_create_pad_link(&unicam->sensor->entity, 0,
1033 -                                   &unicam->video_dev.entity, 0,
1034 -                                   MEDIA_LNK_FL_ENABLED |
1035 -                                   MEDIA_LNK_FL_IMMUTABLE);
1036 +       ret = v4l2_device_register_ro_subdev_nodes(&unicam->v4l2_dev);
1037         if (ret) {
1038 -               unicam_err(unicam, "Unable to create pad links.\n");
1039 -               video_unregister_device(&unicam->video_dev);
1040 -               return ret;
1041 +               unicam_err(unicam, "Unable to register subdev nodes.\n");
1042 +               goto unregister;
1043         }
1044
1045         return 0;
1046 +
1047 +unregister:
1048 +       unregister_nodes(unicam);
1049 +
1050 +       return ret;
1051  }
1052
1053  static int unicam_async_complete(struct v4l2_async_notifier *notifier)
1054 @@ -2274,7 +2355,8 @@ static int unicam_probe(struct platform_
1055                  pdev->dev.driver->name, dev_name(&pdev->dev));
1056         unicam->mdev.hw_revision = 1;
1057
1058 -       media_entity_pads_init(&unicam->video_dev.entity, 1, &unicam->pad);
1059 +       media_entity_pads_init(&unicam->node[0].video_dev.entity, 1,
1060 +                              &unicam->node[0].pad);
1061         media_device_init(&unicam->mdev);
1062
1063         unicam->v4l2_dev.mdev = &unicam->mdev;
1064 @@ -2294,7 +2376,7 @@ static int unicam_probe(struct platform_
1065         }
1066
1067         /* Reserve space for the controls */
1068 -       hdl = &unicam->ctrl_handler;
1069 +       hdl = &unicam->node[0].ctrl_handler;
1070         ret = v4l2_ctrl_handler_init(hdl, 16);
1071         if (ret < 0)
1072                 goto media_unregister;
1073 @@ -2335,9 +2417,9 @@ static int unicam_remove(struct platform
1074         pm_runtime_disable(&pdev->dev);
1075
1076         v4l2_async_notifier_unregister(&unicam->notifier);
1077 -       v4l2_ctrl_handler_free(&unicam->ctrl_handler);
1078 +       v4l2_ctrl_handler_free(&unicam->node[0].ctrl_handler);
1079         v4l2_device_unregister(&unicam->v4l2_dev);
1080 -       video_unregister_device(&unicam->video_dev);
1081 +       unregister_nodes(unicam);
1082         if (unicam->sensor_config)
1083                 v4l2_subdev_free_pad_config(unicam->sensor_config);
1084         media_device_unregister(&unicam->mdev);