target/linux/bcm27xx/patches-5.15/950-0214-media-bcm2835-unicam-Add-embedded-data-node.patch

   1 From 85fc685aabde594162e398c048b901d31332c2e6 Mon Sep 17 00:00:00 2001
   2 From: Naushir Patuck <naush@raspberrypi.com>
   3 Date: Thu, 16 Apr 2020 11:35:41 +0100
   4 Subject: [PATCH] media: bcm2835-unicam: Add embedded data node.
   5
   6 This patch adds a new node in the bcm2835-unicam driver to support
   7 CSI-2 embedded data streams.  The subdevice is queried to see if
   8 embedded data is available from the sensor.
   9
  10 Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
  11 ---
  12  .../media/platform/bcm2835/bcm2835-unicam.c   | 667 +++++++++++++-----
  13  1 file changed, 474 insertions(+), 193 deletions(-)
  14
  15 --- a/drivers/media/platform/bcm2835/bcm2835-unicam.c
  16 +++ b/drivers/media/platform/bcm2835/bcm2835-unicam.c
  17 @@ -109,8 +109,15 @@ MODULE_PARM_DESC(debug, "Debug level 0-3
  18  /* Define a nominal minimum image size */
  19  #define MIN_WIDTH      16
  20  #define MIN_HEIGHT     16
  21 -/* Maximum number of simulataneous streams Uncaim can handle. */
  22 -#define MAX_NODES      2
  23 +/* Default size of the embedded buffer */
  24 +#define UNICAM_EMBEDDED_SIZE   8192
  25 +
  26 +enum pad_types {
  27 +       IMAGE_PAD,
  28 +       METADATA_PAD,
  29 +       MAX_NODES
  30 +};
  31 +
  32  /*
  33   * struct unicam_fmt - Unicam media bus format information
  34   * @pixelformat: V4L2 pixel format FCC identifier. 0 if n/a.
  35 @@ -327,6 +334,12 @@ static const struct unicam_fmt formats[]
  36                 .depth          = 12,
  37                 .csi_dt         = 0x2c,
  38         },
  39 +       /* Embedded data format */
  40 +       {
  41 +               .fourcc         = V4L2_META_FMT_SENSOR_DATA,
  42 +               .code           = MEDIA_BUS_FMT_SENSOR_DATA,
  43 +               .depth          = 8,
  44 +       }
  45  };
  46
  47  struct unicam_dmaqueue {
  48 @@ -348,7 +361,9 @@ struct unicam_cfg {
  49  #define MAX_POSSIBLE_PIX_FMTS (ARRAY_SIZE(formats))
  50
  51  struct unicam_node {
  52 -       bool registered;
  53 +       int registered;
  54 +       int open;
  55 +       int streaming;
  56         unsigned int pad_id;
  57         /* Pointer pointing to current v4l2_buffer */
  58         struct unicam_buffer *cur_frm;
  59 @@ -374,6 +389,7 @@ struct unicam_node {
  60         struct unicam_device *dev;
  61         struct media_pad pad;
  62         struct v4l2_ctrl_handler ctrl_handler;
  63 +       unsigned int embedded_lines;
  64  };
  65
  66  struct unicam_device {
  67 @@ -401,8 +417,6 @@ struct unicam_device {
  68         struct v4l2_subdev *sensor;
  69         /* Pad config for the sensor */
  70         struct v4l2_subdev_pad_config *sensor_config;
  71 -       /* current input at the sub device */
  72 -       int current_input;
  73
  74         unsigned int virtual_channel;
  75         enum v4l2_mbus_type bus_type;
  76 @@ -413,10 +427,7 @@ struct unicam_device {
  77         unsigned int bus_flags;
  78         unsigned int max_data_lanes;
  79         unsigned int active_data_lanes;
  80 -
  81 -       struct v4l2_rect crop;
  82 -       /* Flag to denote that we are processing buffers */
  83 -       int streaming;
  84 +       bool sensor_embedded_data;
  85
  86         struct unicam_node node[MAX_NODES];
  87  };
  88 @@ -488,6 +499,7 @@ static int check_mbus_format(struct unic
  89         for (i = 0; !ret && i < MAX_ENUM_MBUS_CODE; i++) {
  90                 memset(&mbus_code, 0, sizeof(mbus_code));
  91                 mbus_code.index = i;
  92 +               mbus_code.pad = IMAGE_PAD;
  93                 mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
  94
  95                 ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code,
  96 @@ -552,10 +564,11 @@ static int __subdev_get_format(struct un
  97  }
  98
  99  static int __subdev_set_format(struct unicam_device *dev,
 100 -                              struct v4l2_mbus_framefmt *fmt)
 101 +                              struct v4l2_mbus_framefmt *fmt, int pad_id)
 102  {
 103         struct v4l2_subdev_format sd_fmt = {
 104                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 105 +               .pad = pad_id
 106         };
 107         int ret;
 108
 109 @@ -566,8 +579,12 @@ static int __subdev_set_format(struct un
 110         if (ret < 0)
 111                 return ret;
 112
 113 -       unicam_dbg(1, dev, "%s %dx%d code:%04x\n", __func__,
 114 -                  fmt->width, fmt->height, fmt->code);
 115 +       if (pad_id == IMAGE_PAD)
 116 +               unicam_dbg(1, dev, "%s %dx%d code:%04x\n", __func__, fmt->width,
 117 +                          fmt->height, fmt->code);
 118 +       else
 119 +               unicam_dbg(1, dev, "%s Embedded data code:%04x\n", __func__,
 120 +                          sd_fmt.format.code);
 121
 122         return 0;
 123  }
 124 @@ -609,46 +626,70 @@ static int unicam_reset_format(struct un
 125         struct v4l2_mbus_framefmt mbus_fmt;
 126         int ret;
 127
 128 -       ret = __subdev_get_format(dev, &mbus_fmt, node->pad_id);
 129 -       if (ret) {
 130 -               unicam_err(dev, "Failed to get_format - ret %d\n", ret);
 131 -               return ret;
 132 -       }
 133 +       if (dev->sensor_embedded_data || node->pad_id != METADATA_PAD) {
 134 +               ret = __subdev_get_format(dev, &mbus_fmt, node->pad_id);
 135 +               if (ret) {
 136 +                       unicam_err(dev, "Failed to get_format - ret %d\n", ret);
 137 +                       return ret;
 138 +               }
 139
 140 -       if (mbus_fmt.code != dev->node[0].fmt->code) {
 141 -               unicam_err(dev, "code mismatch - fmt->code %08x, mbus_fmt.code %08x\n",
 142 -                          dev->node[0].fmt->code, mbus_fmt.code);
 143 -               return ret;
 144 +               if (mbus_fmt.code != node->fmt->code) {
 145 +                       unicam_err(dev, "code mismatch - fmt->code %08x, mbus_fmt.code %08x\n",
 146 +                                  node->fmt->code, mbus_fmt.code);
 147 +                       return ret;
 148 +               }
 149         }
 150
 151 -       v4l2_fill_pix_format(&dev->node[0].v_fmt.fmt.pix, &mbus_fmt);
 152 -       dev->node[0].v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 153 -
 154 -       unicam_calc_format_size_bpl(dev, dev->node[0].fmt, &dev->node[0].v_fmt);
 155 -
 156 -       dev->node[0].m_fmt = mbus_fmt;
 157 +       if (node->pad_id == IMAGE_PAD) {
 158 +               v4l2_fill_pix_format(&node->v_fmt.fmt.pix, &mbus_fmt);
 159 +               node->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 160 +               unicam_calc_format_size_bpl(dev, node->fmt, &node->v_fmt);
 161 +       } else {
 162 +               node->v_fmt.type = V4L2_BUF_TYPE_META_CAPTURE;
 163 +               node->v_fmt.fmt.meta.dataformat = V4L2_META_FMT_SENSOR_DATA;
 164 +               if (dev->sensor_embedded_data) {
 165 +                       node->v_fmt.fmt.meta.buffersize =
 166 +                                       mbus_fmt.width * mbus_fmt.height;
 167 +                       node->embedded_lines = mbus_fmt.height;
 168 +               } else {
 169 +                       node->v_fmt.fmt.meta.buffersize = UNICAM_EMBEDDED_SIZE;
 170 +                       node->embedded_lines = 1;
 171 +               }
 172 +       }
 173
 174 +       node->m_fmt = mbus_fmt;
 175         return 0;
 176  }
 177
 178 -static void unicam_wr_dma_addr(struct unicam_device *dev, dma_addr_t dmaaddr)
 179 +static void unicam_wr_dma_addr(struct unicam_device *dev, dma_addr_t dmaaddr,
 180 +                              int pad_id)
 181  {
 182 +       dma_addr_t endaddr;
 183 +
 184         /*
 185          * dmaaddr should be a 32-bit address with the top two bits set to 0x3
 186          * to signify uncached access through the Videocore memory controller.
 187          */
 188         BUG_ON((dmaaddr >> 30) != 0x3);
 189
 190 -       reg_write(&dev->cfg, UNICAM_IBSA0, dmaaddr);
 191 -       reg_write(&dev->cfg, UNICAM_IBEA0,
 192 -                 dmaaddr + dev->node[0].v_fmt.fmt.pix.sizeimage);
 193 +       if (pad_id == IMAGE_PAD) {
 194 +               endaddr = dmaaddr +
 195 +                         dev->node[IMAGE_PAD].v_fmt.fmt.pix.sizeimage;
 196 +               reg_write(&dev->cfg, UNICAM_IBSA0, dmaaddr);
 197 +               reg_write(&dev->cfg, UNICAM_IBEA0, endaddr);
 198 +       } else {
 199 +               endaddr = dmaaddr +
 200 +                         dev->node[METADATA_PAD].v_fmt.fmt.meta.buffersize;
 201 +               reg_write(&dev->cfg, UNICAM_DBSA0, dmaaddr);
 202 +               reg_write(&dev->cfg, UNICAM_DBEA0, endaddr);
 203 +       }
 204  }
 205
 206  static inline unsigned int unicam_get_lines_done(struct unicam_device *dev)
 207  {
 208         dma_addr_t start_addr, cur_addr;
 209 -       unsigned int stride = dev->node[0].v_fmt.fmt.pix.bytesperline;
 210 -       struct unicam_buffer *frm = dev->node[0].cur_frm;
 211 +       unsigned int stride = dev->node[IMAGE_PAD].v_fmt.fmt.pix.bytesperline;
 212 +       struct unicam_buffer *frm = dev->node[IMAGE_PAD].cur_frm;
 213
 214         if (!frm)
 215                 return 0;
 216 @@ -658,27 +699,51 @@ static inline unsigned int unicam_get_li
 217         return (unsigned int)(cur_addr - start_addr) / stride;
 218  }
 219
 220 -static inline void unicam_schedule_next_buffer(struct unicam_device *dev)
 221 +static inline void unicam_schedule_next_buffer(struct unicam_node *node)
 222  {
 223 -       struct unicam_dmaqueue *dma_q = &dev->node[0].dma_queue;
 224 +       struct unicam_device *dev = node->dev;
 225 +       struct unicam_dmaqueue *dma_q = &node->dma_queue;
 226         struct unicam_buffer *buf;
 227         dma_addr_t addr;
 228
 229         buf = list_entry(dma_q->active.next, struct unicam_buffer, list);
 230 -       dev->node[0].next_frm = buf;
 231 +       node->next_frm = buf;
 232         list_del(&buf->list);
 233
 234         addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
 235 -       unicam_wr_dma_addr(dev, addr);
 236 +       unicam_wr_dma_addr(dev, addr, node->pad_id);
 237  }
 238
 239 -static inline void unicam_process_buffer_complete(struct unicam_device *dev)
 240 +static inline void unicam_process_buffer_complete(struct unicam_node *node,
 241 +                                                 unsigned int sequence)
 242  {
 243 -       dev->node[0].cur_frm->vb.field = dev->node[0].m_fmt.field;
 244 -       dev->node[0].cur_frm->vb.sequence = dev->sequence++;
 245 +       node->cur_frm->vb.field = node->m_fmt.field;
 246 +       node->cur_frm->vb.sequence = sequence;
 247 +
 248 +       vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
 249 +       node->cur_frm = node->next_frm;
 250 +}
 251
 252 -       vb2_buffer_done(&dev->node[0].cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
 253 -       dev->node[0].cur_frm = dev->node[0].next_frm;
 254 +static int unicam_num_nodes_streaming(struct unicam_device *dev)
 255 +{
 256 +       return dev->node[IMAGE_PAD].streaming +
 257 +              dev->node[METADATA_PAD].streaming;
 258 +}
 259 +
 260 +static int unicam_all_nodes_streaming(struct unicam_device *dev)
 261 +{
 262 +       int ret;
 263 +
 264 +       ret = dev->node[IMAGE_PAD].open && dev->node[IMAGE_PAD].streaming;
 265 +       ret &= !dev->node[METADATA_PAD].open ||
 266 +              dev->node[METADATA_PAD].streaming;
 267 +       return ret;
 268 +}
 269 +
 270 +static int unicam_all_nodes_disabled(struct unicam_device *dev)
 271 +{
 272 +       return !dev->node[IMAGE_PAD].streaming &&
 273 +              !dev->node[METADATA_PAD].streaming;
 274  }
 275
 276  /*
 277 @@ -693,10 +758,12 @@ static irqreturn_t unicam_isr(int irq, v
 278  {
 279         struct unicam_device *unicam = (struct unicam_device *)dev;
 280         struct unicam_cfg *cfg = &unicam->cfg;
 281 -       struct unicam_dmaqueue *dma_q = &unicam->node[0].dma_queue;
 282         unsigned int lines_done = unicam_get_lines_done(dev);
 283         unsigned int sequence = unicam->sequence;
 284 +       int num_nodes_streaming = unicam_num_nodes_streaming(dev);
 285         int ista, sta;
 286 +       u64 ts;
 287 +       int i;
 288
 289         /*
 290          * Don't service interrupts if not streaming.
 291 @@ -704,7 +771,7 @@ static irqreturn_t unicam_isr(int irq, v
 292          * peripheral without the kernel knowing (that
 293          * shouldn't happen, but causes issues if it does).
 294          */
 295 -       if (!unicam->streaming)
 296 +       if (unicam_all_nodes_disabled(unicam))
 297                 return IRQ_HANDLED;
 298
 299         sta = reg_read(cfg, UNICAM_STA);
 300 @@ -726,9 +793,12 @@ static irqreturn_t unicam_isr(int irq, v
 301                  * Timestamp is to be when the first data byte was captured,
 302                  * aka frame start.
 303                  */
 304 -               if (unicam->node[0].cur_frm)
 305 -                       unicam->node[0].cur_frm->vb.vb2_buf.timestamp =
 306 -                               ktime_get_ns();
 307 +               ts = ktime_get_ns();
 308 +               for (i = 0; i < num_nodes_streaming; i++) {
 309 +                       if (unicam->node[i].cur_frm)
 310 +                               unicam->node[i].cur_frm->vb.vb2_buf.timestamp =
 311 +                                                               ts;
 312 +               }
 313         }
 314         if (ista & UNICAM_FEI || sta & UNICAM_PI0) {
 315                 /*
 316 @@ -736,9 +806,13 @@ static irqreturn_t unicam_isr(int irq, v
 317                  * stop the peripheral. Overwrite the frame we've just
 318                  * captured instead.
 319                  */
 320 -               if (unicam->node[0].cur_frm &&
 321 -                   unicam->node[0].cur_frm != unicam->node[0].next_frm)
 322 -                       unicam_process_buffer_complete(unicam);
 323 +               for (i = 0; i < num_nodes_streaming; i++) {
 324 +                       if (unicam->node[i].cur_frm &&
 325 +                           unicam->node[i].cur_frm != unicam->node[i].next_frm)
 326 +                               unicam_process_buffer_complete(&unicam->node[i],
 327 +                                                              sequence);
 328 +               }
 329 +               unicam->sequence++;
 330         }
 331
 332         /* Cannot swap buffer at frame end, there may be a race condition
 333 @@ -746,11 +820,13 @@ static irqreturn_t unicam_isr(int irq, v
 334          * already started.
 335          */
 336         if (ista & (UNICAM_FSI | UNICAM_LCI) && !(ista & UNICAM_FEI)) {
 337 -               spin_lock(&unicam->node[0].dma_queue_lock);
 338 -               if (!list_empty(&dma_q->active) &&
 339 -                   unicam->node[0].cur_frm == unicam->node[0].next_frm)
 340 -                       unicam_schedule_next_buffer(unicam);
 341 -               spin_unlock(&unicam->node[0].dma_queue_lock);
 342 +               for (i = 0; i < num_nodes_streaming; i++) {
 343 +                       spin_lock(&unicam->node[i].dma_queue_lock);
 344 +                       if (!list_empty(&unicam->node[i].dma_queue.active) &&
 345 +                           unicam->node[i].cur_frm == unicam->node[i].next_frm)
 346 +                               unicam_schedule_next_buffer(&unicam->node[i]);
 347 +                       spin_unlock(&unicam->node[i].dma_queue_lock);
 348 +               }
 349         }
 350
 351         if (reg_read(&unicam->cfg, UNICAM_ICTL) & UNICAM_FCM) {
 352 @@ -773,6 +849,15 @@ static int unicam_querycap(struct file *
 353         snprintf(cap->bus_info, sizeof(cap->bus_info),
 354                  "platform:%s", dev->v4l2_dev.name);
 355
 356 +       cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
 357 +                           V4L2_CAP_READWRITE | V4L2_CAP_DEVICE_CAPS |
 358 +                           V4L2_CAP_META_CAPTURE;
 359 +
 360 +       if (node->pad_id == IMAGE_PAD)
 361 +               cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 362 +       else
 363 +               cap->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING;
 364 +
 365         return 0;
 366  }
 367
 368 @@ -787,9 +872,14 @@ static int unicam_enum_fmt_vid_cap(struc
 369         int ret = 0;
 370         int i;
 371
 372 +       if (node->pad_id == METADATA_PAD)
 373 +               return -EINVAL;
 374 +
 375         for (i = 0; !ret && i < MAX_ENUM_MBUS_CODE; i++) {
 376                 memset(&mbus_code, 0, sizeof(mbus_code));
 377                 mbus_code.index = i;
 378 +               mbus_code.pad = IMAGE_PAD;
 379 +               mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 380
 381                 ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code,
 382                                        NULL, &mbus_code);
 383 @@ -827,6 +917,9 @@ static int unicam_g_fmt_vid_cap(struct f
 384  {
 385         struct unicam_node *node = video_drvdata(file);
 386
 387 +       if (node->pad_id == METADATA_PAD)
 388 +               return -EINVAL;
 389 +
 390         *f = node->v_fmt;
 391
 392         return 0;
 393 @@ -843,6 +936,9 @@ const struct unicam_fmt *get_first_suppo
 394         for (j = 0; ret != -EINVAL && ret != -ENOIOCTLCMD; ++j) {
 395                 memset(&mbus_code, 0, sizeof(mbus_code));
 396                 mbus_code.index = j;
 397 +               mbus_code.pad = IMAGE_PAD;
 398 +               mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 399 +
 400                 ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, NULL,
 401                                        &mbus_code);
 402                 if (ret < 0) {
 403 @@ -873,12 +969,15 @@ static int unicam_try_fmt_vid_cap(struct
 404         struct unicam_device *dev = node->dev;
 405         struct v4l2_subdev_format sd_fmt = {
 406                 .which = V4L2_SUBDEV_FORMAT_TRY,
 407 -               .pad = 0
 408 +               .pad = IMAGE_PAD
 409         };
 410         struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
 411         const struct unicam_fmt *fmt;
 412         int ret;
 413
 414 +       if (node->pad_id == METADATA_PAD)
 415 +               return -EINVAL;
 416 +
 417         fmt = find_format_by_pix(dev, f->fmt.pix.pixelformat);
 418         if (!fmt) {
 419                 /* Pixel format not supported by unicam. Choose the first
 420 @@ -983,7 +1082,7 @@ static int unicam_s_fmt_vid_cap(struct f
 421
 422         v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, fmt->code);
 423
 424 -       ret = __subdev_set_format(dev, &mbus_fmt);
 425 +       ret = __subdev_set_format(dev, &mbus_fmt, node->pad_id);
 426         if (ret) {
 427                 unicam_dbg(3, dev, "%s __subdev_set_format failed %d\n",
 428                            __func__, ret);
 429 @@ -1014,6 +1113,106 @@ static int unicam_s_fmt_vid_cap(struct f
 430         return 0;
 431  }
 432
 433 +static int unicam_enum_fmt_meta_cap(struct file *file, void *priv,
 434 +                                   struct v4l2_fmtdesc *f)
 435 +{
 436 +       struct unicam_node *node = video_drvdata(file);
 437 +       struct unicam_device *dev = node->dev;
 438 +       struct v4l2_subdev_mbus_code_enum mbus_code;
 439 +       const struct unicam_fmt *fmt = NULL;
 440 +       int ret = 0;
 441 +
 442 +       if (node->pad_id != METADATA_PAD || f->index != 0)
 443 +               return -EINVAL;
 444 +
 445 +       if (dev->sensor_embedded_data) {
 446 +               memset(&mbus_code, 0, sizeof(mbus_code));
 447 +               mbus_code.index = f->index;
 448 +               mbus_code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 449 +               mbus_code.pad = METADATA_PAD;
 450 +
 451 +               ret = v4l2_subdev_call(dev->sensor, pad, enum_mbus_code, NULL,
 452 +                                      &mbus_code);
 453 +               if (ret < 0) {
 454 +                       unicam_dbg(2, dev,
 455 +                                  "subdev->enum_mbus_code idx 0 returned %d - index invalid\n",
 456 +                                  ret);
 457 +                       return -EINVAL;
 458 +               }
 459 +       } else {
 460 +               mbus_code.code = MEDIA_BUS_FMT_SENSOR_DATA;
 461 +       }
 462 +
 463 +       fmt = find_format_by_code(mbus_code.code);
 464 +       if (fmt)
 465 +               f->pixelformat = fmt->fourcc;
 466 +
 467 +       return 0;
 468 +}
 469 +
 470 +static int unicam_g_fmt_meta_cap(struct file *file, void *priv,
 471 +                                struct v4l2_format *f)
 472 +{
 473 +       struct unicam_node *node = video_drvdata(file);
 474 +
 475 +       if (node->pad_id != METADATA_PAD)
 476 +               return -EINVAL;
 477 +
 478 +       *f = node->v_fmt;
 479 +
 480 +       return 0;
 481 +}
 482 +
 483 +static int unicam_try_fmt_meta_cap(struct file *file, void *priv,
 484 +                                  struct v4l2_format *f)
 485 +{
 486 +       struct unicam_node *node = video_drvdata(file);
 487 +
 488 +       if (node->pad_id != METADATA_PAD)
 489 +               return -EINVAL;
 490 +
 491 +       *f = node->v_fmt;
 492 +
 493 +       return 0;
 494 +}
 495 +
 496 +static int unicam_s_fmt_meta_cap(struct file *file, void *priv,
 497 +                                struct v4l2_format *f)
 498 +{
 499 +       struct unicam_node *node = video_drvdata(file);
 500 +       struct unicam_device *dev = node->dev;
 501 +       struct v4l2_mbus_framefmt mbus_fmt = { 0 };
 502 +       const struct unicam_fmt *fmt;
 503 +       int ret;
 504 +
 505 +       if (node->pad_id == IMAGE_PAD)
 506 +               return -EINVAL;
 507 +
 508 +       if (dev->sensor_embedded_data) {
 509 +               fmt = find_format_by_pix(dev, f->fmt.meta.dataformat);
 510 +               if (!fmt) {
 511 +                       unicam_err(dev, "unknown format: V4L2 pix 0x%08x\n",
 512 +                                  f->fmt.meta.dataformat);
 513 +                       return -EINVAL;
 514 +               }
 515 +               mbus_fmt.code = fmt->code;
 516 +               ret = __subdev_set_format(dev, &mbus_fmt, node->pad_id);
 517 +               if (ret) {
 518 +                       unicam_dbg(3, dev, "%s __subdev_set_format failed %d\n",
 519 +                                  __func__, ret);
 520 +                       return ret;
 521 +               }
 522 +       }
 523 +
 524 +       *f = node->v_fmt;
 525 +
 526 +       unicam_dbg(3, dev, "%s size %d, V4L2 pix 0x%08x\n",
 527 +                  __func__, node->v_fmt.fmt.meta.buffersize,
 528 +                  node->v_fmt.fmt.meta.dataformat);
 529 +
 530 +       return 0;
 531 +}
 532 +
 533  static int unicam_queue_setup(struct vb2_queue *vq,
 534                               unsigned int *nbuffers,
 535                               unsigned int *nplanes,
 536 @@ -1022,7 +1221,9 @@ static int unicam_queue_setup(struct vb2
 537  {
 538         struct unicam_node *node = vb2_get_drv_priv(vq);
 539         struct unicam_device *dev = node->dev;
 540 -       unsigned int size = node->v_fmt.fmt.pix.sizeimage;
 541 +       unsigned int size = node->pad_id == IMAGE_PAD ?
 542 +                                   node->v_fmt.fmt.pix.sizeimage :
 543 +                                   node->v_fmt.fmt.meta.buffersize;
 544
 545         if (vq->num_buffers + *nbuffers < 3)
 546                 *nbuffers = 3 - vq->num_buffers;
 547 @@ -1053,7 +1254,8 @@ static int unicam_buffer_prepare(struct
 548         if (WARN_ON(!node->fmt))
 549                 return -EINVAL;
 550
 551 -       size = node->v_fmt.fmt.pix.sizeimage;
 552 +       size = node->pad_id == IMAGE_PAD ? node->v_fmt.fmt.pix.sizeimage :
 553 +                                          node->v_fmt.fmt.meta.buffersize;
 554         if (vb2_plane_size(vb, 0) < size) {
 555                 unicam_err(dev, "data will not fit into plane (%lu < %lu)\n",
 556                            vb2_plane_size(vb, 0), size);
 557 @@ -1082,12 +1284,12 @@ static void unicam_set_packing_config(st
 558         int pack, unpack;
 559         u32 val;
 560
 561 -       if (dev->node[0].v_fmt.fmt.pix.pixelformat ==
 562 -           dev->node[0].fmt->fourcc) {
 563 +       if (dev->node[IMAGE_PAD].v_fmt.fmt.pix.pixelformat ==
 564 +           dev->node[IMAGE_PAD].fmt->fourcc) {
 565                 unpack = UNICAM_PUM_NONE;
 566                 pack = UNICAM_PPM_NONE;
 567         } else {
 568 -               switch (dev->node[0].fmt->depth) {
 569 +               switch (dev->node[IMAGE_PAD].fmt->depth) {
 570                 case 8:
 571                         unpack = UNICAM_PUM_UNPACK8;
 572                         break;
 573 @@ -1125,17 +1327,31 @@ static void unicam_cfg_image_id(struct u
 574         if (dev->bus_type == V4L2_MBUS_CSI2_DPHY) {
 575                 /* CSI2 mode */
 576                 reg_write(cfg, UNICAM_IDI0,
 577 -                       (dev->virtual_channel << 6) | dev->node[0].fmt->csi_dt);
 578 +                         (dev->virtual_channel << 6) |
 579 +                                             dev->node[IMAGE_PAD].fmt->csi_dt);
 580         } else {
 581                 /* CCP2 mode */
 582 -               reg_write(cfg, UNICAM_IDI0, (0x80 | dev->node[0].fmt->csi_dt));
 583 +               reg_write(cfg, UNICAM_IDI0,
 584 +                         0x80 | dev->node[IMAGE_PAD].fmt->csi_dt);
 585         }
 586  }
 587
 588 -static void unicam_start_rx(struct unicam_device *dev, unsigned long addr)
 589 +static void unicam_enable_ed(struct unicam_device *dev)
 590 +{
 591 +       struct unicam_cfg *cfg = &dev->cfg;
 592 +       u32 val = reg_read(cfg, UNICAM_DCS);
 593 +
 594 +       set_field(&val, 2, UNICAM_EDL_MASK);
 595 +       /* Do not wrap at the end of the embedded data buffer */
 596 +       set_field(&val, 0, UNICAM_DBOB);
 597 +
 598 +       reg_write(cfg, UNICAM_DCS, val);
 599 +}
 600 +
 601 +static void unicam_start_rx(struct unicam_device *dev, dma_addr_t *addr)
 602  {
 603         struct unicam_cfg *cfg = &dev->cfg;
 604 -       int line_int_freq = dev->node[0].v_fmt.fmt.pix.height >> 2;
 605 +       int line_int_freq = dev->node[IMAGE_PAD].v_fmt.fmt.pix.height >> 2;
 606         unsigned int i;
 607         u32 val;
 608
 609 @@ -1284,27 +1500,31 @@ static void unicam_start_rx(struct unica
 610         }
 611
 612         reg_write(&dev->cfg, UNICAM_IBLS,
 613 -                 dev->node[0].v_fmt.fmt.pix.bytesperline);
 614 -       unicam_wr_dma_addr(dev, addr);
 615 +                 dev->node[IMAGE_PAD].v_fmt.fmt.pix.bytesperline);
 616 +       unicam_wr_dma_addr(dev, addr[IMAGE_PAD], IMAGE_PAD);
 617         unicam_set_packing_config(dev);
 618         unicam_cfg_image_id(dev);
 619
 620 -       /* Disabled embedded data */
 621 -       val = 0;
 622 -       set_field(&val, 0, UNICAM_EDL_MASK);
 623 -       reg_write(cfg, UNICAM_DCS, val);
 624 -
 625         val = reg_read(cfg, UNICAM_MISC);
 626         set_field(&val, 1, UNICAM_FL0);
 627         set_field(&val, 1, UNICAM_FL1);
 628         reg_write(cfg, UNICAM_MISC, val);
 629
 630 +       if (dev->node[METADATA_PAD].streaming && dev->sensor_embedded_data) {
 631 +               unicam_enable_ed(dev);
 632 +               unicam_wr_dma_addr(dev, addr[METADATA_PAD], METADATA_PAD);
 633 +       }
 634 +
 635         /* Enable peripheral */
 636         reg_write_field(cfg, UNICAM_CTRL, 1, UNICAM_CPE);
 637
 638         /* Load image pointers */
 639         reg_write_field(cfg, UNICAM_ICTL, 1, UNICAM_LIP_MASK);
 640
 641 +       /* Load embedded data buffer pointers if needed */
 642 +       if (dev->node[METADATA_PAD].streaming && dev->sensor_embedded_data)
 643 +               reg_write_field(cfg, UNICAM_DCS, 1, UNICAM_LDP);
 644 +
 645         /*
 646          * Enable trigger only for the first frame to
 647          * sync correctly to the FS from the source.
 648 @@ -1339,6 +1559,9 @@ static void unicam_disable(struct unicam
 649         /* Disable peripheral */
 650         reg_write_field(cfg, UNICAM_CTRL, 0, UNICAM_CPE);
 651
 652 +       /* Clear ED setup */
 653 +       reg_write(cfg, UNICAM_DCS, 0);
 654 +
 655         /* Disable all lane clocks */
 656         clk_write(cfg, 0);
 657  }
 658 @@ -1347,26 +1570,23 @@ static int unicam_start_streaming(struct
 659  {
 660         struct unicam_node *node = vb2_get_drv_priv(vq);
 661         struct unicam_device *dev = node->dev;
 662 -       struct unicam_dmaqueue *dma_q = &node->dma_queue;
 663 -       struct unicam_buffer *buf, *tmp;
 664 -       unsigned long addr = 0;
 665 +       struct unicam_buffer *buf;
 666 +       dma_addr_t buffer_addr[MAX_NODES] = { 0 };
 667 +       int num_nodes_streaming;
 668         unsigned long flags;
 669 -       int ret;
 670 +       int ret, i;
 671
 672 -       spin_lock_irqsave(&node->dma_queue_lock, flags);
 673 -       buf = list_entry(dma_q->active.next, struct unicam_buffer, list);
 674 -       node->cur_frm = buf;
 675 -       node->next_frm = buf;
 676 -       list_del(&buf->list);
 677 -       spin_unlock_irqrestore(&node->dma_queue_lock, flags);
 678 +       node->streaming = 1;
 679 +       if (!unicam_all_nodes_streaming(dev)) {
 680 +               unicam_dbg(3, dev, "Not all nodes are streaming yet.");
 681 +               return 0;
 682 +       }
 683
 684 -       addr = vb2_dma_contig_plane_dma_addr(&node->cur_frm->vb.vb2_buf, 0);
 685         dev->sequence = 0;
 686 -
 687         ret = unicam_runtime_get(dev);
 688         if (ret < 0) {
 689                 unicam_dbg(3, dev, "unicam_runtime_get failed\n");
 690 -               goto err_release_buffers;
 691 +               return ret;
 692         }
 693
 694         dev->active_data_lanes = dev->max_data_lanes;
 695 @@ -1388,7 +1608,7 @@ static int unicam_start_streaming(struct
 696                         dev->active_data_lanes = dev->max_data_lanes;
 697         }
 698         if (dev->active_data_lanes > dev->max_data_lanes) {
 699 -               unicam_err(dev, "Device has requested %u data lanes, which is >%u configured in DT\n",
 700 +               unicam_err(dev, "Device has requested %u data lanes, which is >%u configured in DT\n",
 701                            dev->active_data_lanes, dev->max_data_lanes);
 702                 ret = -EINVAL;
 703                 goto err_pm_put;
 704 @@ -1408,9 +1628,22 @@ static int unicam_start_streaming(struct
 705                 unicam_err(dev, "Failed to enable CSI clock: %d\n", ret);
 706                 goto err_pm_put;
 707         }
 708 -       dev->streaming = 1;
 709
 710 -       unicam_start_rx(dev, addr);
 711 +       num_nodes_streaming = unicam_num_nodes_streaming(dev);
 712 +       for (i = 0; i < num_nodes_streaming; i++) {
 713 +               spin_lock_irqsave(&dev->node[i].dma_queue_lock, flags);
 714 +               buf = list_entry(dev->node[i].dma_queue.active.next,
 715 +                                struct unicam_buffer, list);
 716 +               dev->node[i].cur_frm = buf;
 717 +               dev->node[i].next_frm = buf;
 718 +               list_del(&buf->list);
 719 +               spin_unlock_irqrestore(&dev->node[i].dma_queue_lock, flags);
 720 +               buffer_addr[i] =
 721 +               vb2_dma_contig_plane_dma_addr(&dev->node[i].cur_frm->vb.vb2_buf,
 722 +                                             0);
 723 +       }
 724 +
 725 +       unicam_start_rx(dev, buffer_addr);
 726
 727         ret = v4l2_subdev_call(dev->sensor, video, s_stream, 1);
 728         if (ret < 0) {
 729 @@ -1421,21 +1654,11 @@ static int unicam_start_streaming(struct
 730         return 0;
 731
 732  err_disable_unicam:
 733 +       node->streaming = 0;
 734         unicam_disable(dev);
 735         clk_disable_unprepare(dev->clock);
 736  err_pm_put:
 737         unicam_runtime_put(dev);
 738 -err_release_buffers:
 739 -       list_for_each_entry_safe(buf, tmp, &dma_q->active, list) {
 740 -               list_del(&buf->list);
 741 -               vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 742 -       }
 743 -       if (node->cur_frm != node->next_frm)
 744 -               vb2_buffer_done(&node->next_frm->vb.vb2_buf,
 745 -                               VB2_BUF_STATE_QUEUED);
 746 -       vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 747 -       node->next_frm = NULL;
 748 -       node->cur_frm = NULL;
 749
 750         return ret;
 751  }
 752 @@ -1448,33 +1671,47 @@ static void unicam_stop_streaming(struct
 753         struct unicam_buffer *buf, *tmp;
 754         unsigned long flags;
 755
 756 -       if (v4l2_subdev_call(dev->sensor, video, s_stream, 0) < 0)
 757 -               unicam_err(dev, "stream off failed in subdev\n");
 758 +       node->streaming = 0;
 759
 760 -       unicam_disable(dev);
 761 +       if (node->pad_id == IMAGE_PAD) {
 762 +               /* Stop streaming the sensor and disable the peripheral.
 763 +                * We cannot continue streaming embedded data with the
 764 +                * image pad disabled.
 765 +                */
 766 +               if (v4l2_subdev_call(dev->sensor, video, s_stream, 0) < 0)
 767 +                       unicam_err(dev, "stream off failed in subdev\n");
 768
 769 -       /* Release all active buffers */
 770 +               unicam_disable(dev);
 771 +               clk_disable_unprepare(dev->clock);
 772 +               unicam_runtime_put(dev);
 773 +
 774 +       } else if (node->pad_id == METADATA_PAD) {
 775 +               /* Null out the embedded data buffer address so the HW does
 776 +                * not use it.  This is only really needed if the embedded data
 777 +                * pad is disabled before the image pad.  The 0x3 in the top two
 778 +                * bits signifies uncached accesses through the Videocore
 779 +                * memory controller.
 780 +                */
 781 +               unicam_wr_dma_addr(dev, 0xc0000000, METADATA_PAD);
 782 +       }
 783 +
 784 +       /* Clear all queued buffers for the node */
 785         spin_lock_irqsave(&node->dma_queue_lock, flags);
 786         list_for_each_entry_safe(buf, tmp, &dma_q->active, list) {
 787                 list_del(&buf->list);
 788                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 789         }
 790
 791 -       if (node->cur_frm == node->next_frm) {
 792 -               vb2_buffer_done(&node->cur_frm->vb.vb2_buf,
 793 -                               VB2_BUF_STATE_ERROR);
 794 -       } else {
 795 +       if (node->cur_frm)
 796                 vb2_buffer_done(&node->cur_frm->vb.vb2_buf,
 797                                 VB2_BUF_STATE_ERROR);
 798 +       if (node->next_frm && node->cur_frm != node->next_frm)
 799                 vb2_buffer_done(&node->next_frm->vb.vb2_buf,
 800                                 VB2_BUF_STATE_ERROR);
 801 -       }
 802 +
 803         node->cur_frm = NULL;
 804         node->next_frm = NULL;
 805         spin_unlock_irqrestore(&node->dma_queue_lock, flags);
 806 -
 807 -       clk_disable_unprepare(dev->clock);
 808 -       unicam_runtime_put(dev);
 809  }
 810
 811  static int unicam_enum_input(struct file *file, void *priv,
 812 @@ -1595,17 +1832,23 @@ static int unicam_enum_framesizes(struct
 813         struct v4l2_subdev_frame_size_enum fse;
 814         int ret;
 815
 816 -       /* check for valid format */
 817 -       fmt = find_format_by_pix(dev, fsize->pixel_format);
 818 -       if (!fmt) {
 819 -               unicam_dbg(3, dev, "Invalid pixel code: %x\n",
 820 -                          fsize->pixel_format);
 821 -               return -EINVAL;
 822 +       if (node->pad_id == IMAGE_PAD) {
 823 +               /* check for valid format */
 824 +               fmt = find_format_by_pix(dev, fsize->pixel_format);
 825 +               if (!fmt) {
 826 +                       unicam_dbg(3, dev, "Invalid pixel code: %x\n",
 827 +                                  fsize->pixel_format);
 828 +                       return -EINVAL;
 829 +               }
 830 +               fse.code = fmt->code;
 831 +       } else {
 832 +               /* This pad is for embedded data, so just set the format */
 833 +               fse.code = MEDIA_BUS_FMT_SENSOR_DATA;
 834         }
 835
 836 +       fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 837         fse.index = fsize->index;
 838 -       fse.pad = 0;
 839 -       fse.code = fmt->code;
 840 +       fse.pad = node->pad_id;
 841
 842         ret = v4l2_subdev_call(dev->sensor, pad, enum_frame_size, NULL, &fse);
 843         if (ret)
 844 @@ -1782,7 +2025,7 @@ static void unicam_notify(struct v4l2_su
 845
 846         switch (notification) {
 847         case V4L2_DEVICE_NOTIFY_EVENT:
 848 -               v4l2_event_queue(&dev->node[0].video_dev, arg);
 849 +               v4l2_event_queue(&dev->node[IMAGE_PAD].video_dev, arg);
 850                 break;
 851         default:
 852                 break;
 853 @@ -1826,6 +2069,7 @@ static int unicam_open(struct file *file
 854                 goto unlock;
 855         }
 856
 857 +       node->open++;
 858         ret = 0;
 859
 860  unlock:
 861 @@ -1850,6 +2094,10 @@ static int unicam_release(struct file *f
 862         if (fh_singular)
 863                 v4l2_subdev_call(sd, core, s_power, 0);
 864
 865 +       if (node->streaming)
 866 +               unicam_stop_streaming(&node->buffer_queue);
 867 +
 868 +       node->open--;
 869         mutex_unlock(&node->lock);
 870
 871         return ret;
 872 @@ -1874,6 +2122,11 @@ static const struct v4l2_ioctl_ops unica
 873         .vidioc_s_fmt_vid_cap           = unicam_s_fmt_vid_cap,
 874         .vidioc_try_fmt_vid_cap         = unicam_try_fmt_vid_cap,
 875
 876 +       .vidioc_enum_fmt_meta_cap       = unicam_enum_fmt_meta_cap,
 877 +       .vidioc_g_fmt_meta_cap          = unicam_g_fmt_meta_cap,
 878 +       .vidioc_s_fmt_meta_cap          = unicam_s_fmt_meta_cap,
 879 +       .vidioc_try_fmt_meta_cap        = unicam_try_fmt_meta_cap,
 880 +
 881         .vidioc_enum_input              = unicam_enum_input,
 882         .vidioc_g_input                 = unicam_g_input,
 883         .vidioc_s_input                 = unicam_s_input,
 884 @@ -1941,42 +2194,53 @@ static int register_node(struct unicam_d
 885         const struct unicam_fmt *fmt;
 886         int ret;
 887
 888 -       ret = __subdev_get_format(unicam, &mbus_fmt, pad_id);
 889 -       if (ret) {
 890 -               unicam_err(unicam, "Failed to get_format - ret %d\n", ret);
 891 -               return ret;
 892 -       }
 893 -
 894 -       fmt = find_format_by_code(mbus_fmt.code);
 895 -       if (!fmt) {
 896 -               /* Find the first format that the sensor and unicam both
 897 -                * support
 898 -                */
 899 -               fmt = get_first_supported_format(unicam);
 900 +       if (unicam->sensor_embedded_data || pad_id != METADATA_PAD) {
 901 +               ret = __subdev_get_format(unicam, &mbus_fmt, pad_id);
 902 +               if (ret) {
 903 +                       unicam_err(unicam, "Failed to get_format - ret %d\n",
 904 +                                  ret);
 905 +                       return ret;
 906 +               }
 907
 908 -               if (!fmt)
 909 -                       /* No compatible formats */
 910 -                       return -EINVAL;
 911 +               fmt = find_format_by_code(mbus_fmt.code);
 912 +               if (!fmt) {
 913 +                       /* Find the first format that the sensor and unicam both
 914 +                        * support
 915 +                        */
 916 +                       fmt = get_first_supported_format(unicam);
 917
 918 -               mbus_fmt.code = fmt->code;
 919 -               ret = __subdev_set_format(unicam, &mbus_fmt);
 920 -               if (ret)
 921 -                       return -EINVAL;
 922 -       }
 923 -       if (mbus_fmt.field != V4L2_FIELD_NONE) {
 924 -               /* Interlaced not supported - disable it now. */
 925 -               mbus_fmt.field = V4L2_FIELD_NONE;
 926 -               ret = __subdev_set_format(unicam, &mbus_fmt);
 927 -               if (ret)
 928 -                       return -EINVAL;
 929 +                       if (!fmt)
 930 +                               /* No compatible formats */
 931 +                               return -EINVAL;
 932 +
 933 +                       mbus_fmt.code = fmt->code;
 934 +                       ret = __subdev_set_format(unicam, &mbus_fmt, pad_id);
 935 +                       if (ret)
 936 +                               return -EINVAL;
 937 +               }
 938 +               if (mbus_fmt.field != V4L2_FIELD_NONE) {
 939 +                       /* Interlaced not supported - disable it now. */
 940 +                       mbus_fmt.field = V4L2_FIELD_NONE;
 941 +                       ret = __subdev_set_format(unicam, &mbus_fmt, pad_id);
 942 +                       if (ret)
 943 +                               return -EINVAL;
 944 +               }
 945 +       } else {
 946 +               /* Fix this node format as embedded data. */
 947 +               fmt = find_format_by_code(MEDIA_BUS_FMT_SENSOR_DATA);
 948         }
 949
 950 +       node->dev = unicam;
 951         node->pad_id = pad_id;
 952         node->fmt = fmt;
 953 -       if (fmt->fourcc)
 954 -               node->v_fmt.fmt.pix.pixelformat = fmt->fourcc;
 955 -       else
 956 +       if (fmt->fourcc) {
 957 +               if (fmt->fourcc != V4L2_META_FMT_SENSOR_DATA)
 958 +                       node->v_fmt.fmt.pix.pixelformat = fmt->fourcc;
 959 +               else
 960 +                       node->v_fmt.fmt.meta.dataformat = fmt->fourcc;
 961 +       } else {
 962                 node->v_fmt.fmt.pix.pixelformat = fmt->repacked_fourcc;
 963 +       }
 964
 965         /* Read current subdev format */
 966         unicam_reset_format(node);
 967 @@ -2002,13 +2266,21 @@ static int register_node(struct unicam_d
 968         spin_lock_init(&node->dma_queue_lock);
 969         mutex_init(&node->lock);
 970
 971 -       if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 972 +       vdev = &node->video_dev;
 973 +       if (pad_id == IMAGE_PAD) {
 974                 /* Add controls from the subdevice */
 975                 ret = v4l2_ctrl_add_handler(&node->ctrl_handler,
 976                                             unicam->sensor->ctrl_handler, NULL,
 977                                             true);
 978                 if (ret < 0)
 979                         return ret;
 980 +
 981 +               /*
 982 +                * If the sensor subdevice has any controls, associate the node
 983 +                *  with the ctrl handler to allow access from userland.
 984 +                */
 985 +               if (!list_empty(&node->ctrl_handler.ctrls))
 986 +                       vdev->ctrl_handler = &node->ctrl_handler;
 987         }
 988
 989         q = &node->buffer_queue;
 990 @@ -2031,8 +2303,6 @@ static int register_node(struct unicam_d
 991
 992         INIT_LIST_HEAD(&node->dma_queue.active);
 993
 994 -       vdev = &node->video_dev;
 995 -       strlcpy(vdev->name, UNICAM_MODULE_NAME, sizeof(vdev->name));
 996         vdev->release = video_device_release_empty;
 997         vdev->fops = &unicam_fops;
 998         vdev->ioctl_ops = &unicam_ioctl_ops;
 999 @@ -2040,24 +2310,28 @@ static int register_node(struct unicam_d
1000         vdev->vfl_dir = VFL_DIR_RX;
1001         vdev->queue = q;
1002         vdev->lock = &node->lock;
1003 -       vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
1004 -                           V4L2_CAP_READWRITE;
1005 -       /* If the source has no controls then remove our ctrl handler. */
1006 -       if (list_empty(&node->ctrl_handler.ctrls))
1007 -               unicam->v4l2_dev.ctrl_handler = NULL;
1008 +       vdev->device_caps = (pad_id == IMAGE_PAD) ?
1009 +                           (V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING) :
1010 +                           (V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING);
1011 +
1012 +       /* Define the device names */
1013 +       snprintf(vdev->name, sizeof(vdev->name), "%s-%s", UNICAM_MODULE_NAME,
1014 +                node->pad_id == IMAGE_PAD ? "image" : "embedded");
1015
1016 -       node->dev = unicam;
1017         video_set_drvdata(vdev, node);
1018         vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT;
1019
1020 -       if (!v4l2_subdev_has_op(unicam->sensor, video, s_std)) {
1021 +       if (node->pad_id == METADATA_PAD ||
1022 +           !v4l2_subdev_has_op(unicam->sensor, video, s_std)) {
1023                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_STD);
1024                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_STD);
1025                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUMSTD);
1026         }
1027 -       if (!v4l2_subdev_has_op(unicam->sensor, video, querystd))
1028 +       if (node->pad_id == METADATA_PAD ||
1029 +           !v4l2_subdev_has_op(unicam->sensor, video, querystd))
1030                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_QUERYSTD);
1031 -       if (!v4l2_subdev_has_op(unicam->sensor, video, s_dv_timings)) {
1032 +       if (node->pad_id == METADATA_PAD ||
1033 +           !v4l2_subdev_has_op(unicam->sensor, video, s_dv_timings)) {
1034                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_EDID);
1035                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_EDID);
1036                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_DV_TIMINGS_CAP);
1037 @@ -2066,15 +2340,19 @@ static int register_node(struct unicam_d
1038                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_DV_TIMINGS);
1039                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_QUERY_DV_TIMINGS);
1040         }
1041 -       if (!v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_interval))
1042 +       if (node->pad_id == METADATA_PAD ||
1043 +           !v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_interval))
1044                 v4l2_disable_ioctl(&node->video_dev,
1045                                    VIDIOC_ENUM_FRAMEINTERVALS);
1046 -       if (!v4l2_subdev_has_op(unicam->sensor, video, g_frame_interval))
1047 +       if (node->pad_id == METADATA_PAD ||
1048 +           !v4l2_subdev_has_op(unicam->sensor, video, g_frame_interval))
1049                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_G_PARM);
1050 -       if (!v4l2_subdev_has_op(unicam->sensor, video, s_frame_interval))
1051 +       if (node->pad_id == METADATA_PAD ||
1052 +           !v4l2_subdev_has_op(unicam->sensor, video, s_frame_interval))
1053                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_S_PARM);
1054
1055 -       if (!v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_size))
1056 +       if (node->pad_id == METADATA_PAD ||
1057 +           !v4l2_subdev_has_op(unicam->sensor, pad, enum_frame_size))
1058                 v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_FRAMESIZES);
1059
1060         ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1061 @@ -2082,27 +2360,29 @@ static int register_node(struct unicam_d
1062                 unicam_err(unicam, "Unable to register video device.\n");
1063                 return ret;
1064         }
1065 -       node->registered = true;
1066 +       node->registered = 1;
1067
1068 -       ret = media_create_pad_link(&unicam->sensor->entity,
1069 -                                   0, &node->video_dev.entity, 0,
1070 -                                   MEDIA_LNK_FL_ENABLED |
1071 -                                   MEDIA_LNK_FL_IMMUTABLE);
1072 -       if (ret)
1073 -               unicam_err(unicam, "Unable to create pad links.\n");
1074 +       if (unicam->sensor_embedded_data) {
1075 +               ret = media_create_pad_link(&unicam->sensor->entity, pad_id,
1076 +                                           &node->video_dev.entity, 0,
1077 +                                           MEDIA_LNK_FL_ENABLED |
1078 +                                           MEDIA_LNK_FL_IMMUTABLE);
1079 +               if (ret)
1080 +                       unicam_err(unicam, "Unable to create pad links.\n");
1081 +       }
1082
1083         return ret;
1084  }
1085
1086  static void unregister_nodes(struct unicam_device *unicam)
1087  {
1088 -       if (unicam->node[0].registered) {
1089 -               video_unregister_device(&unicam->node[0].video_dev);
1090 -               unicam->node[0].registered = false;
1091 -       }
1092 -       if (unicam->node[1].registered) {
1093 -               video_unregister_device(&unicam->node[1].video_dev);
1094 -               unicam->node[1].registered = false;
1095 +       if (unicam->node[IMAGE_PAD].registered) {
1096 +               video_unregister_device(&unicam->node[IMAGE_PAD].video_dev);
1097 +               unicam->node[IMAGE_PAD].registered = 0;
1098 +       }
1099 +       if (unicam->node[METADATA_PAD].registered) {
1100 +               video_unregister_device(&unicam->node[METADATA_PAD].video_dev);
1101 +               unicam->node[METADATA_PAD].registered = 0;
1102         }
1103  }
1104
1105 @@ -2118,20 +2398,20 @@ static int unicam_probe_complete(struct
1106         if (!unicam->sensor_config)
1107                 return -ENOMEM;
1108
1109 -       ret = register_node(unicam, &unicam->node[0],
1110 -                           V4L2_BUF_TYPE_VIDEO_CAPTURE, 0);
1111 +       unicam->sensor_embedded_data = (unicam->sensor->entity.num_pads >= 2);
1112 +
1113 +       ret = register_node(unicam, &unicam->node[IMAGE_PAD],
1114 +                           V4L2_BUF_TYPE_VIDEO_CAPTURE, IMAGE_PAD);
1115         if (ret) {
1116                 unicam_err(unicam, "Unable to register subdev node 0.\n");
1117                 goto unregister;
1118         }
1119 -       if (unicam->sensor->entity.num_pads >= 2) {
1120 -               ret = register_node(unicam, &unicam->node[1],
1121 -                                   V4L2_BUF_TYPE_META_CAPTURE, 1);
1122 -               if (ret) {
1123 -                       unicam_err(unicam,
1124 -                                  "Unable to register subdev node 1.\n");
1125 -                       goto unregister;
1126 -               }
1127 +
1128 +       ret = register_node(unicam, &unicam->node[METADATA_PAD],
1129 +                           V4L2_BUF_TYPE_META_CAPTURE, METADATA_PAD);
1130 +       if (ret) {
1131 +               unicam_err(unicam, "Unable to register subdev node 1.\n");
1132 +               goto unregister;
1133         }
1134
1135         ret = v4l2_device_register_ro_subdev_nodes(&unicam->v4l2_dev);
1136 @@ -2355,8 +2635,10 @@ static int unicam_probe(struct platform_
1137                  pdev->dev.driver->name, dev_name(&pdev->dev));
1138         unicam->mdev.hw_revision = 1;
1139
1140 -       media_entity_pads_init(&unicam->node[0].video_dev.entity, 1,
1141 -                              &unicam->node[0].pad);
1142 +       media_entity_pads_init(&unicam->node[IMAGE_PAD].video_dev.entity, 1,
1143 +                              &unicam->node[IMAGE_PAD].pad);
1144 +       media_entity_pads_init(&unicam->node[METADATA_PAD].video_dev.entity, 1,
1145 +                              &unicam->node[METADATA_PAD].pad);
1146         media_device_init(&unicam->mdev);
1147
1148         unicam->v4l2_dev.mdev = &unicam->mdev;
1149 @@ -2376,11 +2658,10 @@ static int unicam_probe(struct platform_
1150         }
1151
1152         /* Reserve space for the controls */
1153 -       hdl = &unicam->node[0].ctrl_handler;
1154 +       hdl = &unicam->node[IMAGE_PAD].ctrl_handler;
1155         ret = v4l2_ctrl_handler_init(hdl, 16);
1156         if (ret < 0)
1157                 goto media_unregister;
1158 -       unicam->v4l2_dev.ctrl_handler = hdl;
1159
1160         /* set the driver data in platform device */
1161         platform_set_drvdata(pdev, unicam);
1162 @@ -2417,7 +2698,7 @@ static int unicam_remove(struct platform
1163         pm_runtime_disable(&pdev->dev);
1164
1165         v4l2_async_notifier_unregister(&unicam->notifier);
1166 -       v4l2_ctrl_handler_free(&unicam->node[0].ctrl_handler);
1167 +       v4l2_ctrl_handler_free(&unicam->node[IMAGE_PAD].ctrl_handler);
1168         v4l2_device_unregister(&unicam->v4l2_dev);
1169         unregister_nodes(unicam);
1170         if (unicam->sensor_config)