drivers/staging/media/ipu3/ipu3-v4l2.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 // Copyright (C) 2018 Intel Corporation
   3
   4 #include <linux/module.h>
   5 #include <linux/pm_runtime.h>
   6
   7 #include <media/v4l2-event.h>
   8 #include <media/v4l2-ioctl.h>
   9
  10 #include "ipu3.h"
  11 #include "ipu3-dmamap.h"
  12
  13 /******************** v4l2_subdev_ops ********************/
  14
  15 #define IPU3_RUNNING_MODE_VIDEO         0
  16 #define IPU3_RUNNING_MODE_STILL         1
  17
  18 static int imgu_subdev_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
  19 {
  20         struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
  21                                                         struct imgu_v4l2_subdev,
  22                                                         subdev);
  23         struct imgu_device *imgu = v4l2_get_subdevdata(sd);
  24         struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[imgu_sd->pipe];
  25         struct v4l2_rect try_crop = {
  26                 .top = 0,
  27                 .left = 0,
  28         };
  29         unsigned int i;
  30
  31         try_crop.width =
  32                 imgu_pipe->nodes[IMGU_NODE_IN].vdev_fmt.fmt.pix_mp.width;
  33         try_crop.height =
  34                 imgu_pipe->nodes[IMGU_NODE_IN].vdev_fmt.fmt.pix_mp.height;
  35
  36         /* Initialize try_fmt */
  37         for (i = 0; i < IMGU_NODE_NUM; i++) {
  38                 struct v4l2_mbus_framefmt *try_fmt =
  39                         v4l2_subdev_get_try_format(sd, fh->pad, i);
  40
  41                 try_fmt->width = try_crop.width;
  42                 try_fmt->height = try_crop.height;
  43                 try_fmt->code = imgu_pipe->nodes[i].pad_fmt.code;
  44                 try_fmt->field = V4L2_FIELD_NONE;
  45         }
  46
  47         *v4l2_subdev_get_try_crop(sd, fh->pad, IMGU_NODE_IN) = try_crop;
  48         *v4l2_subdev_get_try_compose(sd, fh->pad, IMGU_NODE_IN) = try_crop;
  49
  50         return 0;
  51 }
  52
  53 static int imgu_subdev_s_stream(struct v4l2_subdev *sd, int enable)
  54 {
  55         int i;
  56         unsigned int node;
  57         int r = 0;
  58         struct imgu_device *imgu = v4l2_get_subdevdata(sd);
  59         struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
  60                                                         struct imgu_v4l2_subdev,
  61                                                         subdev);
  62         unsigned int pipe = imgu_sd->pipe;
  63         struct device *dev = &imgu->pci_dev->dev;
  64         struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES] = { NULL };
  65         struct v4l2_rect *rects[IPU3_CSS_RECTS] = { NULL };
  66         struct imgu_css_pipe *css_pipe = &imgu->css.pipes[pipe];
  67         struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
  68
  69         dev_dbg(dev, "%s %d for pipe %u", __func__, enable, pipe);
  70         /* grab ctrl after streamon and return after off */
  71         v4l2_ctrl_grab(imgu_sd->ctrl, enable);
  72
  73         if (!enable) {
  74                 imgu_sd->active = false;
  75                 return 0;
  76         }
  77
  78         for (i = 0; i < IMGU_NODE_NUM; i++)
  79                 imgu_pipe->queue_enabled[i] = imgu_pipe->nodes[i].enabled;
  80
  81         /* This is handled specially */
  82         imgu_pipe->queue_enabled[IPU3_CSS_QUEUE_PARAMS] = false;
  83
  84         /* Initialize CSS formats */
  85         for (i = 0; i < IPU3_CSS_QUEUES; i++) {
  86                 node = imgu_map_node(imgu, i);
  87                 /* No need to reconfig meta nodes */
  88                 if (node == IMGU_NODE_STAT_3A || node == IMGU_NODE_PARAMS)
  89                         continue;
  90                 fmts[i] = imgu_pipe->queue_enabled[node] ?
  91                         &imgu_pipe->nodes[node].vdev_fmt.fmt.pix_mp : NULL;
  92         }
  93
  94         /* Enable VF output only when VF queue requested by user */
  95         css_pipe->vf_output_en = false;
  96         if (imgu_pipe->nodes[IMGU_NODE_VF].enabled)
  97                 css_pipe->vf_output_en = true;
  98
  99         if (atomic_read(&imgu_sd->running_mode) == IPU3_RUNNING_MODE_VIDEO)
 100                 css_pipe->pipe_id = IPU3_CSS_PIPE_ID_VIDEO;
 101         else
 102                 css_pipe->pipe_id = IPU3_CSS_PIPE_ID_CAPTURE;
 103
 104         dev_dbg(dev, "IPU3 pipe %u pipe_id %u", pipe, css_pipe->pipe_id);
 105
 106         rects[IPU3_CSS_RECT_EFFECTIVE] = &imgu_sd->rect.eff;
 107         rects[IPU3_CSS_RECT_BDS] = &imgu_sd->rect.bds;
 108         rects[IPU3_CSS_RECT_GDC] = &imgu_sd->rect.gdc;
 109
 110         r = imgu_css_fmt_set(&imgu->css, fmts, rects, pipe);
 111         if (r) {
 112                 dev_err(dev, "failed to set initial formats pipe %u with (%d)",
 113                         pipe, r);
 114                 return r;
 115         }
 116
 117         imgu_sd->active = true;
 118
 119         return 0;
 120 }
 121
 122 static int imgu_subdev_get_fmt(struct v4l2_subdev *sd,
 123                                struct v4l2_subdev_pad_config *cfg,
 124                                struct v4l2_subdev_format *fmt)
 125 {
 126         struct imgu_device *imgu = v4l2_get_subdevdata(sd);
 127         struct v4l2_mbus_framefmt *mf;
 128         struct imgu_media_pipe *imgu_pipe;
 129         u32 pad = fmt->pad;
 130         struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
 131                                                         struct imgu_v4l2_subdev,
 132                                                         subdev);
 133         unsigned int pipe = imgu_sd->pipe;
 134
 135         imgu_pipe = &imgu->imgu_pipe[pipe];
 136         if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 137                 fmt->format = imgu_pipe->nodes[pad].pad_fmt;
 138         } else {
 139                 mf = v4l2_subdev_get_try_format(sd, cfg, pad);
 140                 fmt->format = *mf;
 141         }
 142
 143         return 0;
 144 }
 145
 146 static int imgu_subdev_set_fmt(struct v4l2_subdev *sd,
 147                                struct v4l2_subdev_pad_config *cfg,
 148                                struct v4l2_subdev_format *fmt)
 149 {
 150         struct imgu_media_pipe *imgu_pipe;
 151         struct imgu_device *imgu = v4l2_get_subdevdata(sd);
 152         struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
 153                                                         struct imgu_v4l2_subdev,
 154                                                         subdev);
 155
 156         struct v4l2_mbus_framefmt *mf;
 157         u32 pad = fmt->pad;
 158         unsigned int pipe = imgu_sd->pipe;
 159
 160         dev_dbg(&imgu->pci_dev->dev, "set subdev %u pad %u fmt to [%ux%u]",
 161                 pipe, pad, fmt->format.width, fmt->format.height);
 162
 163         imgu_pipe = &imgu->imgu_pipe[pipe];
 164         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
 165                 mf = v4l2_subdev_get_try_format(sd, cfg, pad);
 166         else
 167                 mf = &imgu_pipe->nodes[pad].pad_fmt;
 168
 169         fmt->format.code = mf->code;
 170         /* Clamp the w and h based on the hardware capabilities */
 171         if (imgu_sd->subdev_pads[pad].flags & MEDIA_PAD_FL_SOURCE) {
 172                 fmt->format.width = clamp(fmt->format.width,
 173                                           IPU3_OUTPUT_MIN_WIDTH,
 174                                           IPU3_OUTPUT_MAX_WIDTH);
 175                 fmt->format.height = clamp(fmt->format.height,
 176                                            IPU3_OUTPUT_MIN_HEIGHT,
 177                                            IPU3_OUTPUT_MAX_HEIGHT);
 178         } else {
 179                 fmt->format.width = clamp(fmt->format.width,
 180                                           IPU3_INPUT_MIN_WIDTH,
 181                                           IPU3_INPUT_MAX_WIDTH);
 182                 fmt->format.height = clamp(fmt->format.height,
 183                                            IPU3_INPUT_MIN_HEIGHT,
 184                                            IPU3_INPUT_MAX_HEIGHT);
 185         }
 186
 187         *mf = fmt->format;
 188
 189         return 0;
 190 }
 191
 192 static int imgu_subdev_get_selection(struct v4l2_subdev *sd,
 193                                      struct v4l2_subdev_pad_config *cfg,
 194                                      struct v4l2_subdev_selection *sel)
 195 {
 196         struct v4l2_rect *try_sel, *r;
 197         struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
 198                                                         struct imgu_v4l2_subdev,
 199                                                         subdev);
 200
 201         if (sel->pad != IMGU_NODE_IN)
 202                 return -EINVAL;
 203
 204         switch (sel->target) {
 205         case V4L2_SEL_TGT_CROP:
 206                 try_sel = v4l2_subdev_get_try_crop(sd, cfg, sel->pad);
 207                 r = &imgu_sd->rect.eff;
 208                 break;
 209         case V4L2_SEL_TGT_COMPOSE:
 210                 try_sel = v4l2_subdev_get_try_compose(sd, cfg, sel->pad);
 211                 r = &imgu_sd->rect.bds;
 212                 break;
 213         default:
 214                 return -EINVAL;
 215         }
 216
 217         if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
 218                 sel->r = *try_sel;
 219         else
 220                 sel->r = *r;
 221
 222         return 0;
 223 }
 224
 225 static int imgu_subdev_set_selection(struct v4l2_subdev *sd,
 226                                      struct v4l2_subdev_pad_config *cfg,
 227                                      struct v4l2_subdev_selection *sel)
 228 {
 229         struct imgu_device *imgu = v4l2_get_subdevdata(sd);
 230         struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
 231                                                         struct imgu_v4l2_subdev,
 232                                                         subdev);
 233         struct v4l2_rect *rect, *try_sel;
 234
 235         dev_dbg(&imgu->pci_dev->dev,
 236                  "set subdev %u sel which %u target 0x%4x rect [%ux%u]",
 237                  imgu_sd->pipe, sel->which, sel->target,
 238                  sel->r.width, sel->r.height);
 239
 240         if (sel->pad != IMGU_NODE_IN)
 241                 return -EINVAL;
 242
 243         switch (sel->target) {
 244         case V4L2_SEL_TGT_CROP:
 245                 try_sel = v4l2_subdev_get_try_crop(sd, cfg, sel->pad);
 246                 rect = &imgu_sd->rect.eff;
 247                 break;
 248         case V4L2_SEL_TGT_COMPOSE:
 249                 try_sel = v4l2_subdev_get_try_compose(sd, cfg, sel->pad);
 250                 rect = &imgu_sd->rect.bds;
 251                 break;
 252         default:
 253                 return -EINVAL;
 254         }
 255
 256         if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
 257                 *try_sel = sel->r;
 258         else
 259                 *rect = sel->r;
 260
 261         return 0;
 262 }
 263
 264 /******************** media_entity_operations ********************/
 265
 266 static int imgu_link_setup(struct media_entity *entity,
 267                            const struct media_pad *local,
 268                            const struct media_pad *remote, u32 flags)
 269 {
 270         struct imgu_media_pipe *imgu_pipe;
 271         struct v4l2_subdev *sd = container_of(entity, struct v4l2_subdev,
 272                                               entity);
 273         struct imgu_device *imgu = v4l2_get_subdevdata(sd);
 274         struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
 275                                                         struct imgu_v4l2_subdev,
 276                                                         subdev);
 277         unsigned int pipe = imgu_sd->pipe;
 278         u32 pad = local->index;
 279
 280         WARN_ON(pad >= IMGU_NODE_NUM);
 281
 282         dev_dbg(&imgu->pci_dev->dev, "pipe %u pad %u is %s", pipe, pad,
 283                  flags & MEDIA_LNK_FL_ENABLED ? "enabled" : "disabled");
 284
 285         imgu_pipe = &imgu->imgu_pipe[pipe];
 286         imgu_pipe->nodes[pad].enabled = flags & MEDIA_LNK_FL_ENABLED;
 287
 288         /* enable input node to enable the pipe */
 289         if (pad != IMGU_NODE_IN)
 290                 return 0;
 291
 292         if (flags & MEDIA_LNK_FL_ENABLED)
 293                 __set_bit(pipe, imgu->css.enabled_pipes);
 294         else
 295                 __clear_bit(pipe, imgu->css.enabled_pipes);
 296
 297         dev_dbg(&imgu->pci_dev->dev, "pipe %u is %s", pipe,
 298                  flags & MEDIA_LNK_FL_ENABLED ? "enabled" : "disabled");
 299
 300         return 0;
 301 }
 302
 303 /******************** vb2_ops ********************/
 304
 305 static int imgu_vb2_buf_init(struct vb2_buffer *vb)
 306 {
 307         struct sg_table *sg = vb2_dma_sg_plane_desc(vb, 0);
 308         struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
 309         struct imgu_buffer *buf = container_of(vb,
 310                 struct imgu_buffer, vid_buf.vbb.vb2_buf);
 311         struct imgu_video_device *node =
 312                 container_of(vb->vb2_queue, struct imgu_video_device, vbq);
 313         unsigned int queue = imgu_node_to_queue(node->id);
 314
 315         if (queue == IPU3_CSS_QUEUE_PARAMS)
 316                 return 0;
 317
 318         return imgu_dmamap_map_sg(imgu, sg->sgl, sg->nents, &buf->map);
 319 }
 320
 321 /* Called when each buffer is freed */
 322 static void imgu_vb2_buf_cleanup(struct vb2_buffer *vb)
 323 {
 324         struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
 325         struct imgu_buffer *buf = container_of(vb,
 326                 struct imgu_buffer, vid_buf.vbb.vb2_buf);
 327         struct imgu_video_device *node =
 328                 container_of(vb->vb2_queue, struct imgu_video_device, vbq);
 329         unsigned int queue = imgu_node_to_queue(node->id);
 330
 331         if (queue == IPU3_CSS_QUEUE_PARAMS)
 332                 return;
 333
 334         imgu_dmamap_unmap(imgu, &buf->map);
 335 }
 336
 337 /* Transfer buffer ownership to me */
 338 static void imgu_vb2_buf_queue(struct vb2_buffer *vb)
 339 {
 340         struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
 341         struct imgu_video_device *node =
 342                 container_of(vb->vb2_queue, struct imgu_video_device, vbq);
 343         unsigned int queue = imgu_node_to_queue(node->id);
 344         struct imgu_buffer *buf = container_of(vb, struct imgu_buffer,
 345                                                vid_buf.vbb.vb2_buf);
 346         unsigned long need_bytes;
 347         unsigned long payload = vb2_get_plane_payload(vb, 0);
 348
 349         if (vb->vb2_queue->type == V4L2_BUF_TYPE_META_CAPTURE ||
 350             vb->vb2_queue->type == V4L2_BUF_TYPE_META_OUTPUT)
 351                 need_bytes = node->vdev_fmt.fmt.meta.buffersize;
 352         else
 353                 need_bytes = node->vdev_fmt.fmt.pix_mp.plane_fmt[0].sizeimage;
 354
 355         if (queue == IPU3_CSS_QUEUE_PARAMS && payload && payload < need_bytes) {
 356                 dev_err(&imgu->pci_dev->dev, "invalid data size for params.");
 357                 vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 358                 return;
 359         }
 360
 361         mutex_lock(&imgu->lock);
 362         if (queue != IPU3_CSS_QUEUE_PARAMS)
 363                 imgu_css_buf_init(&buf->css_buf, queue, buf->map.daddr);
 364
 365         list_add_tail(&buf->vid_buf.list, &node->buffers);
 366         mutex_unlock(&imgu->lock);
 367
 368         vb2_set_plane_payload(vb, 0, need_bytes);
 369
 370         if (imgu->streaming)
 371                 imgu_queue_buffers(imgu, false, node->pipe);
 372
 373         dev_dbg(&imgu->pci_dev->dev, "%s for pipe %u node %u", __func__,
 374                 node->pipe, node->id);
 375 }
 376
 377 static int imgu_vb2_queue_setup(struct vb2_queue *vq,
 378                                 unsigned int *num_buffers,
 379                                 unsigned int *num_planes,
 380                                 unsigned int sizes[],
 381                                 struct device *alloc_devs[])
 382 {
 383         struct imgu_device *imgu = vb2_get_drv_priv(vq);
 384         struct imgu_video_device *node =
 385                 container_of(vq, struct imgu_video_device, vbq);
 386         const struct v4l2_format *fmt = &node->vdev_fmt;
 387         unsigned int size;
 388
 389         *num_buffers = clamp_val(*num_buffers, 1, VB2_MAX_FRAME);
 390         alloc_devs[0] = &imgu->pci_dev->dev;
 391
 392         if (vq->type == V4L2_BUF_TYPE_META_CAPTURE ||
 393             vq->type == V4L2_BUF_TYPE_META_OUTPUT)
 394                 size = fmt->fmt.meta.buffersize;
 395         else
 396                 size = fmt->fmt.pix_mp.plane_fmt[0].sizeimage;
 397
 398         if (*num_planes) {
 399                 if (sizes[0] < size)
 400                         return -EINVAL;
 401                 size = sizes[0];
 402         }
 403
 404         *num_planes = 1;
 405         sizes[0] = size;
 406
 407         /* Initialize buffer queue */
 408         INIT_LIST_HEAD(&node->buffers);
 409
 410         return 0;
 411 }
 412
 413 /* Check if all enabled video nodes are streaming, exception ignored */
 414 static bool imgu_all_nodes_streaming(struct imgu_device *imgu,
 415                                      struct imgu_video_device *except)
 416 {
 417         unsigned int i, pipe, p;
 418         struct imgu_video_device *node;
 419         struct device *dev = &imgu->pci_dev->dev;
 420
 421         pipe = except->pipe;
 422         if (!test_bit(pipe, imgu->css.enabled_pipes)) {
 423                 dev_warn(&imgu->pci_dev->dev,
 424                          "pipe %u link is not ready yet", pipe);
 425                 return false;
 426         }
 427
 428         for_each_set_bit(p, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
 429                 for (i = 0; i < IMGU_NODE_NUM; i++) {
 430                         node = &imgu->imgu_pipe[p].nodes[i];
 431                         dev_dbg(dev, "%s pipe %u queue %u name %s enabled = %u",
 432                                 __func__, p, i, node->name, node->enabled);
 433                         if (node == except)
 434                                 continue;
 435                         if (node->enabled && !vb2_start_streaming_called(&node->vbq))
 436                                 return false;
 437                 }
 438         }
 439
 440         return true;
 441 }
 442
 443 static void imgu_return_all_buffers(struct imgu_device *imgu,
 444                                     struct imgu_video_device *node,
 445                                     enum vb2_buffer_state state)
 446 {
 447         struct imgu_vb2_buffer *b, *b0;
 448
 449         /* Return all buffers */
 450         mutex_lock(&imgu->lock);
 451         list_for_each_entry_safe(b, b0, &node->buffers, list) {
 452                 list_del(&b->list);
 453                 vb2_buffer_done(&b->vbb.vb2_buf, state);
 454         }
 455         mutex_unlock(&imgu->lock);
 456 }
 457
 458 static int imgu_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
 459 {
 460         struct imgu_media_pipe *imgu_pipe;
 461         struct imgu_device *imgu = vb2_get_drv_priv(vq);
 462         struct device *dev = &imgu->pci_dev->dev;
 463         struct imgu_video_device *node =
 464                 container_of(vq, struct imgu_video_device, vbq);
 465         int r;
 466         unsigned int pipe;
 467
 468         dev_dbg(dev, "%s node name %s pipe %u id %u", __func__,
 469                 node->name, node->pipe, node->id);
 470
 471         if (imgu->streaming) {
 472                 r = -EBUSY;
 473                 goto fail_return_bufs;
 474         }
 475
 476         if (!node->enabled) {
 477                 dev_err(dev, "IMGU node is not enabled");
 478                 r = -EINVAL;
 479                 goto fail_return_bufs;
 480         }
 481
 482         pipe = node->pipe;
 483         imgu_pipe = &imgu->imgu_pipe[pipe];
 484         r = media_pipeline_start(&node->vdev.entity, &imgu_pipe->pipeline);
 485         if (r < 0)
 486                 goto fail_return_bufs;
 487
 488
 489         if (!imgu_all_nodes_streaming(imgu, node))
 490                 return 0;
 491
 492         for_each_set_bit(pipe, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
 493                 r = v4l2_subdev_call(&imgu->imgu_pipe[pipe].imgu_sd.subdev,
 494                                      video, s_stream, 1);
 495                 if (r < 0)
 496                         goto fail_stop_pipeline;
 497         }
 498
 499         /* Start streaming of the whole pipeline now */
 500         dev_dbg(dev, "IMGU streaming is ready to start");
 501         r = imgu_s_stream(imgu, true);
 502         if (!r)
 503                 imgu->streaming = true;
 504
 505         return 0;
 506
 507 fail_stop_pipeline:
 508         media_pipeline_stop(&node->vdev.entity);
 509 fail_return_bufs:
 510         imgu_return_all_buffers(imgu, node, VB2_BUF_STATE_QUEUED);
 511
 512         return r;
 513 }
 514
 515 static void imgu_vb2_stop_streaming(struct vb2_queue *vq)
 516 {
 517         struct imgu_media_pipe *imgu_pipe;
 518         struct imgu_device *imgu = vb2_get_drv_priv(vq);
 519         struct device *dev = &imgu->pci_dev->dev;
 520         struct imgu_video_device *node =
 521                 container_of(vq, struct imgu_video_device, vbq);
 522         int r;
 523         unsigned int pipe;
 524
 525         WARN_ON(!node->enabled);
 526
 527         pipe = node->pipe;
 528         dev_dbg(dev, "Try to stream off node [%u][%u]", pipe, node->id);
 529         imgu_pipe = &imgu->imgu_pipe[pipe];
 530         r = v4l2_subdev_call(&imgu_pipe->imgu_sd.subdev, video, s_stream, 0);
 531         if (r)
 532                 dev_err(&imgu->pci_dev->dev,
 533                         "failed to stop subdev streaming\n");
 534
 535         /* Was this the first node with streaming disabled? */
 536         if (imgu->streaming && imgu_all_nodes_streaming(imgu, node)) {
 537                 /* Yes, really stop streaming now */
 538                 dev_dbg(dev, "IMGU streaming is ready to stop");
 539                 r = imgu_s_stream(imgu, false);
 540                 if (!r)
 541                         imgu->streaming = false;
 542         }
 543
 544         imgu_return_all_buffers(imgu, node, VB2_BUF_STATE_ERROR);
 545         media_pipeline_stop(&node->vdev.entity);
 546 }
 547
 548 /******************** v4l2_ioctl_ops ********************/
 549
 550 #define VID_CAPTURE     0
 551 #define VID_OUTPUT      1
 552 #define DEF_VID_CAPTURE 0
 553 #define DEF_VID_OUTPUT  1
 554
 555 struct imgu_fmt {
 556         u32     fourcc;
 557         u16     type; /* VID_CAPTURE or VID_OUTPUT not both */
 558 };
 559
 560 /* format descriptions for capture and preview */
 561 static const struct imgu_fmt formats[] = {
 562         { V4L2_PIX_FMT_NV12, VID_CAPTURE },
 563         { V4L2_PIX_FMT_IPU3_SGRBG10, VID_OUTPUT },
 564         { V4L2_PIX_FMT_IPU3_SBGGR10, VID_OUTPUT },
 565         { V4L2_PIX_FMT_IPU3_SGBRG10, VID_OUTPUT },
 566         { V4L2_PIX_FMT_IPU3_SRGGB10, VID_OUTPUT },
 567 };
 568
 569 /* Find the first matched format, return default if not found */
 570 static const struct imgu_fmt *find_format(struct v4l2_format *f, u32 type)
 571 {
 572         unsigned int i;
 573
 574         for (i = 0; i < ARRAY_SIZE(formats); i++) {
 575                 if (formats[i].fourcc == f->fmt.pix_mp.pixelformat &&
 576                     formats[i].type == type)
 577                         return &formats[i];
 578         }
 579
 580         return type == VID_CAPTURE ? &formats[DEF_VID_CAPTURE] :
 581                                      &formats[DEF_VID_OUTPUT];
 582 }
 583
 584 static int imgu_vidioc_querycap(struct file *file, void *fh,
 585                                 struct v4l2_capability *cap)
 586 {
 587         struct imgu_video_device *node = file_to_intel_imgu_node(file);
 588
 589         strscpy(cap->driver, IMGU_NAME, sizeof(cap->driver));
 590         strscpy(cap->card, IMGU_NAME, sizeof(cap->card));
 591         snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s", node->name);
 592
 593         return 0;
 594 }
 595
 596 static int enum_fmts(struct v4l2_fmtdesc *f, u32 type)
 597 {
 598         unsigned int i, j;
 599
 600         for (i = j = 0; i < ARRAY_SIZE(formats); ++i) {
 601                 if (formats[i].type == type) {
 602                         if (j == f->index)
 603                                 break;
 604                         ++j;
 605                 }
 606         }
 607
 608         if (i < ARRAY_SIZE(formats)) {
 609                 f->pixelformat = formats[i].fourcc;
 610                 return 0;
 611         }
 612
 613         return -EINVAL;
 614 }
 615
 616 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 617                                    struct v4l2_fmtdesc *f)
 618 {
 619         if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 620                 return -EINVAL;
 621
 622         return enum_fmts(f, VID_CAPTURE);
 623 }
 624
 625 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 626                                    struct v4l2_fmtdesc *f)
 627 {
 628         if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 629                 return -EINVAL;
 630
 631         return enum_fmts(f, VID_OUTPUT);
 632 }
 633
 634 /* Propagate forward always the format from the CIO2 subdev */
 635 static int imgu_vidioc_g_fmt(struct file *file, void *fh,
 636                              struct v4l2_format *f)
 637 {
 638         struct imgu_video_device *node = file_to_intel_imgu_node(file);
 639
 640         f->fmt = node->vdev_fmt.fmt;
 641
 642         return 0;
 643 }
 644
 645 /*
 646  * Set input/output format. Unless it is just a try, this also resets
 647  * selections (ie. effective and BDS resolutions) to defaults.
 648  */
 649 static int imgu_fmt(struct imgu_device *imgu, unsigned int pipe, int node,
 650                     struct v4l2_format *f, bool try)
 651 {
 652         struct device *dev = &imgu->pci_dev->dev;
 653         struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES] = { NULL };
 654         struct v4l2_rect *rects[IPU3_CSS_RECTS] = { NULL };
 655         struct v4l2_mbus_framefmt pad_fmt;
 656         unsigned int i, css_q;
 657         int ret;
 658         struct imgu_css_pipe *css_pipe = &imgu->css.pipes[pipe];
 659         struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
 660         struct imgu_v4l2_subdev *imgu_sd = &imgu_pipe->imgu_sd;
 661
 662         dev_dbg(dev, "set fmt node [%u][%u](try = %u)", pipe, node, try);
 663
 664         for (i = 0; i < IMGU_NODE_NUM; i++)
 665                 dev_dbg(dev, "IMGU pipe %u node %u enabled = %u",
 666                         pipe, i, imgu_pipe->nodes[i].enabled);
 667
 668         if (imgu_pipe->nodes[IMGU_NODE_VF].enabled)
 669                 css_pipe->vf_output_en = true;
 670
 671         if (atomic_read(&imgu_sd->running_mode) == IPU3_RUNNING_MODE_VIDEO)
 672                 css_pipe->pipe_id = IPU3_CSS_PIPE_ID_VIDEO;
 673         else
 674                 css_pipe->pipe_id = IPU3_CSS_PIPE_ID_CAPTURE;
 675
 676         dev_dbg(dev, "IPU3 pipe %u pipe_id = %u", pipe, css_pipe->pipe_id);
 677
 678         for (i = 0; i < IPU3_CSS_QUEUES; i++) {
 679                 unsigned int inode = imgu_map_node(imgu, i);
 680
 681                 /* Skip the meta node */
 682                 if (inode == IMGU_NODE_STAT_3A || inode == IMGU_NODE_PARAMS)
 683                         continue;
 684
 685                 if (try) {
 686                         fmts[i] = kmemdup(&imgu_pipe->nodes[inode].vdev_fmt.fmt.pix_mp,
 687                                           sizeof(struct v4l2_pix_format_mplane),
 688                                           GFP_KERNEL);
 689                         if (!fmts[i]) {
 690                                 ret = -ENOMEM;
 691                                 goto out;
 692                         }
 693                 } else {
 694                         fmts[i] = &imgu_pipe->nodes[inode].vdev_fmt.fmt.pix_mp;
 695                 }
 696
 697                 /* CSS expects some format on OUT queue */
 698                 if (i != IPU3_CSS_QUEUE_OUT &&
 699                     !imgu_pipe->nodes[inode].enabled)
 700                         fmts[i] = NULL;
 701         }
 702
 703         if (!try) {
 704                 /* eff and bds res got by imgu_s_sel */
 705                 struct imgu_v4l2_subdev *imgu_sd = &imgu_pipe->imgu_sd;
 706
 707                 rects[IPU3_CSS_RECT_EFFECTIVE] = &imgu_sd->rect.eff;
 708                 rects[IPU3_CSS_RECT_BDS] = &imgu_sd->rect.bds;
 709                 rects[IPU3_CSS_RECT_GDC] = &imgu_sd->rect.gdc;
 710
 711                 /* suppose that pad fmt was set by subdev s_fmt before */
 712                 pad_fmt = imgu_pipe->nodes[IMGU_NODE_IN].pad_fmt;
 713                 rects[IPU3_CSS_RECT_GDC]->width = pad_fmt.width;
 714                 rects[IPU3_CSS_RECT_GDC]->height = pad_fmt.height;
 715         }
 716
 717         /*
 718          * imgu doesn't set the node to the value given by user
 719          * before we return success from this function, so set it here.
 720          */
 721         css_q = imgu_node_to_queue(node);
 722         if (!fmts[css_q]) {
 723                 ret = -EINVAL;
 724                 goto out;
 725         }
 726         *fmts[css_q] = f->fmt.pix_mp;
 727
 728         if (try)
 729                 ret = imgu_css_fmt_try(&imgu->css, fmts, rects, pipe);
 730         else
 731                 ret = imgu_css_fmt_set(&imgu->css, fmts, rects, pipe);
 732
 733         /* ret is the binary number in the firmware blob */
 734         if (ret < 0)
 735                 goto out;
 736
 737         if (try)
 738                 f->fmt.pix_mp = *fmts[css_q];
 739         else
 740                 f->fmt = imgu_pipe->nodes[node].vdev_fmt.fmt;
 741
 742 out:
 743         if (try) {
 744                 for (i = 0; i < IPU3_CSS_QUEUES; i++)
 745                         kfree(fmts[i]);
 746         }
 747
 748         return ret;
 749 }
 750
 751 static int imgu_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
 752 {
 753         struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
 754         const struct imgu_fmt *fmt;
 755
 756         if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
 757                 fmt = find_format(f, VID_CAPTURE);
 758         else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 759                 fmt = find_format(f, VID_OUTPUT);
 760         else
 761                 return -EINVAL;
 762
 763         pixm->pixelformat = fmt->fourcc;
 764
 765         memset(pixm->plane_fmt[0].reserved, 0,
 766                sizeof(pixm->plane_fmt[0].reserved));
 767
 768         return 0;
 769 }
 770
 771 static int imgu_vidioc_try_fmt(struct file *file, void *fh,
 772                                struct v4l2_format *f)
 773 {
 774         struct imgu_device *imgu = video_drvdata(file);
 775         struct device *dev = &imgu->pci_dev->dev;
 776         struct imgu_video_device *node = file_to_intel_imgu_node(file);
 777         struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 778         int r;
 779
 780         dev_dbg(dev, "%s [%ux%u] for node %u\n", __func__,
 781                 pix_mp->width, pix_mp->height, node->id);
 782
 783         r = imgu_try_fmt(file, fh, f);
 784         if (r)
 785                 return r;
 786
 787         return imgu_fmt(imgu, node->pipe, node->id, f, true);
 788 }
 789
 790 static int imgu_vidioc_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
 791 {
 792         struct imgu_device *imgu = video_drvdata(file);
 793         struct device *dev = &imgu->pci_dev->dev;
 794         struct imgu_video_device *node = file_to_intel_imgu_node(file);
 795         struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 796         int r;
 797
 798         dev_dbg(dev, "%s [%ux%u] for node %u\n", __func__,
 799                 pix_mp->width, pix_mp->height, node->id);
 800
 801         r = imgu_try_fmt(file, fh, f);
 802         if (r)
 803                 return r;
 804
 805         return imgu_fmt(imgu, node->pipe, node->id, f, false);
 806 }
 807
 808 struct imgu_meta_fmt {
 809         __u32 fourcc;
 810         char *name;
 811 };
 812
 813 /* From drivers/media/v4l2-core/v4l2-ioctl.c */
 814 static const struct imgu_meta_fmt meta_fmts[] = {
 815         { V4L2_META_FMT_IPU3_PARAMS, "IPU3 processing parameters" },
 816         { V4L2_META_FMT_IPU3_STAT_3A, "IPU3 3A statistics" },
 817 };
 818
 819 static int imgu_meta_enum_format(struct file *file, void *fh,
 820                                  struct v4l2_fmtdesc *fmt)
 821 {
 822         struct imgu_video_device *node = file_to_intel_imgu_node(file);
 823         unsigned int i = fmt->type == V4L2_BUF_TYPE_META_OUTPUT ? 0 : 1;
 824
 825         /* Each node is dedicated to only one meta format */
 826         if (fmt->index > 0 || fmt->type != node->vbq.type)
 827                 return -EINVAL;
 828
 829         strscpy(fmt->description, meta_fmts[i].name, sizeof(fmt->description));
 830         fmt->pixelformat = meta_fmts[i].fourcc;
 831
 832         return 0;
 833 }
 834
 835 static int imgu_vidioc_g_meta_fmt(struct file *file, void *fh,
 836                                   struct v4l2_format *f)
 837 {
 838         struct imgu_video_device *node = file_to_intel_imgu_node(file);
 839
 840         if (f->type != node->vbq.type)
 841                 return -EINVAL;
 842
 843         f->fmt = node->vdev_fmt.fmt;
 844
 845         return 0;
 846 }
 847
 848 static int imgu_vidioc_enum_input(struct file *file, void *fh,
 849                                   struct v4l2_input *input)
 850 {
 851         if (input->index > 0)
 852                 return -EINVAL;
 853         strscpy(input->name, "camera", sizeof(input->name));
 854         input->type = V4L2_INPUT_TYPE_CAMERA;
 855
 856         return 0;
 857 }
 858
 859 static int imgu_vidioc_g_input(struct file *file, void *fh, unsigned int *input)
 860 {
 861         *input = 0;
 862
 863         return 0;
 864 }
 865
 866 static int imgu_vidioc_s_input(struct file *file, void *fh, unsigned int input)
 867 {
 868         return input == 0 ? 0 : -EINVAL;
 869 }
 870
 871 static int imgu_vidioc_enum_output(struct file *file, void *fh,
 872                                    struct v4l2_output *output)
 873 {
 874         if (output->index > 0)
 875                 return -EINVAL;
 876         strscpy(output->name, "camera", sizeof(output->name));
 877         output->type = V4L2_INPUT_TYPE_CAMERA;
 878
 879         return 0;
 880 }
 881
 882 static int imgu_vidioc_g_output(struct file *file, void *fh,
 883                                 unsigned int *output)
 884 {
 885         *output = 0;
 886
 887         return 0;
 888 }
 889
 890 static int imgu_vidioc_s_output(struct file *file, void *fh,
 891                                 unsigned int output)
 892 {
 893         return output == 0 ? 0 : -EINVAL;
 894 }
 895
 896 /******************** function pointers ********************/
 897
 898 static struct v4l2_subdev_internal_ops imgu_subdev_internal_ops = {
 899         .open = imgu_subdev_open,
 900 };
 901
 902 static const struct v4l2_subdev_core_ops imgu_subdev_core_ops = {
 903         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
 904         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
 905 };
 906
 907 static const struct v4l2_subdev_video_ops imgu_subdev_video_ops = {
 908         .s_stream = imgu_subdev_s_stream,
 909 };
 910
 911 static const struct v4l2_subdev_pad_ops imgu_subdev_pad_ops = {
 912         .link_validate = v4l2_subdev_link_validate_default,
 913         .get_fmt = imgu_subdev_get_fmt,
 914         .set_fmt = imgu_subdev_set_fmt,
 915         .get_selection = imgu_subdev_get_selection,
 916         .set_selection = imgu_subdev_set_selection,
 917 };
 918
 919 static const struct v4l2_subdev_ops imgu_subdev_ops = {
 920         .core = &imgu_subdev_core_ops,
 921         .video = &imgu_subdev_video_ops,
 922         .pad = &imgu_subdev_pad_ops,
 923 };
 924
 925 static const struct media_entity_operations imgu_media_ops = {
 926         .link_setup = imgu_link_setup,
 927         .link_validate = v4l2_subdev_link_validate,
 928 };
 929
 930 /****************** vb2_ops of the Q ********************/
 931
 932 static const struct vb2_ops imgu_vb2_ops = {
 933         .buf_init = imgu_vb2_buf_init,
 934         .buf_cleanup = imgu_vb2_buf_cleanup,
 935         .buf_queue = imgu_vb2_buf_queue,
 936         .queue_setup = imgu_vb2_queue_setup,
 937         .start_streaming = imgu_vb2_start_streaming,
 938         .stop_streaming = imgu_vb2_stop_streaming,
 939         .wait_prepare = vb2_ops_wait_prepare,
 940         .wait_finish = vb2_ops_wait_finish,
 941 };
 942
 943 /****************** v4l2_file_operations *****************/
 944
 945 static const struct v4l2_file_operations imgu_v4l2_fops = {
 946         .unlocked_ioctl = video_ioctl2,
 947         .open = v4l2_fh_open,
 948         .release = vb2_fop_release,
 949         .poll = vb2_fop_poll,
 950         .mmap = vb2_fop_mmap,
 951 };
 952
 953 /******************** v4l2_ioctl_ops ********************/
 954
 955 static const struct v4l2_ioctl_ops imgu_v4l2_ioctl_ops = {
 956         .vidioc_querycap = imgu_vidioc_querycap,
 957
 958         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 959         .vidioc_g_fmt_vid_cap_mplane = imgu_vidioc_g_fmt,
 960         .vidioc_s_fmt_vid_cap_mplane = imgu_vidioc_s_fmt,
 961         .vidioc_try_fmt_vid_cap_mplane = imgu_vidioc_try_fmt,
 962
 963         .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 964         .vidioc_g_fmt_vid_out_mplane = imgu_vidioc_g_fmt,
 965         .vidioc_s_fmt_vid_out_mplane = imgu_vidioc_s_fmt,
 966         .vidioc_try_fmt_vid_out_mplane = imgu_vidioc_try_fmt,
 967
 968         .vidioc_enum_output = imgu_vidioc_enum_output,
 969         .vidioc_g_output = imgu_vidioc_g_output,
 970         .vidioc_s_output = imgu_vidioc_s_output,
 971
 972         .vidioc_enum_input = imgu_vidioc_enum_input,
 973         .vidioc_g_input = imgu_vidioc_g_input,
 974         .vidioc_s_input = imgu_vidioc_s_input,
 975
 976         /* buffer queue management */
 977         .vidioc_reqbufs = vb2_ioctl_reqbufs,
 978         .vidioc_create_bufs = vb2_ioctl_create_bufs,
 979         .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
 980         .vidioc_querybuf = vb2_ioctl_querybuf,
 981         .vidioc_qbuf = vb2_ioctl_qbuf,
 982         .vidioc_dqbuf = vb2_ioctl_dqbuf,
 983         .vidioc_streamon = vb2_ioctl_streamon,
 984         .vidioc_streamoff = vb2_ioctl_streamoff,
 985         .vidioc_expbuf = vb2_ioctl_expbuf,
 986 };
 987
 988 static const struct v4l2_ioctl_ops imgu_v4l2_meta_ioctl_ops = {
 989         .vidioc_querycap = imgu_vidioc_querycap,
 990
 991         /* meta capture */
 992         .vidioc_enum_fmt_meta_cap = imgu_meta_enum_format,
 993         .vidioc_g_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
 994         .vidioc_s_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
 995         .vidioc_try_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
 996
 997         /* meta output */
 998         .vidioc_enum_fmt_meta_out = imgu_meta_enum_format,
 999         .vidioc_g_fmt_meta_out = imgu_vidioc_g_meta_fmt,
1000         .vidioc_s_fmt_meta_out = imgu_vidioc_g_meta_fmt,
1001         .vidioc_try_fmt_meta_out = imgu_vidioc_g_meta_fmt,
1002
1003         .vidioc_reqbufs = vb2_ioctl_reqbufs,
1004         .vidioc_create_bufs = vb2_ioctl_create_bufs,
1005         .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
1006         .vidioc_querybuf = vb2_ioctl_querybuf,
1007         .vidioc_qbuf = vb2_ioctl_qbuf,
1008         .vidioc_dqbuf = vb2_ioctl_dqbuf,
1009         .vidioc_streamon = vb2_ioctl_streamon,
1010         .vidioc_streamoff = vb2_ioctl_streamoff,
1011         .vidioc_expbuf = vb2_ioctl_expbuf,
1012 };
1013
1014 static int imgu_sd_s_ctrl(struct v4l2_ctrl *ctrl)
1015 {
1016         struct imgu_v4l2_subdev *imgu_sd =
1017                 container_of(ctrl->handler, struct imgu_v4l2_subdev, ctrl_handler);
1018         struct imgu_device *imgu = v4l2_get_subdevdata(&imgu_sd->subdev);
1019         struct device *dev = &imgu->pci_dev->dev;
1020
1021         dev_dbg(dev, "set val %d to ctrl 0x%8x for subdev %u",
1022                 ctrl->val, ctrl->id, imgu_sd->pipe);
1023
1024         switch (ctrl->id) {
1025         case V4L2_CID_INTEL_IPU3_MODE:
1026                 atomic_set(&imgu_sd->running_mode, ctrl->val);
1027                 return 0;
1028         default:
1029                 return -EINVAL;
1030         }
1031 }
1032
1033 static const struct v4l2_ctrl_ops imgu_subdev_ctrl_ops = {
1034         .s_ctrl = imgu_sd_s_ctrl,
1035 };
1036
1037 static const char * const imgu_ctrl_mode_strings[] = {
1038         "Video mode",
1039         "Still mode",
1040 };
1041
1042 static const struct v4l2_ctrl_config imgu_subdev_ctrl_mode = {
1043         .ops = &imgu_subdev_ctrl_ops,
1044         .id = V4L2_CID_INTEL_IPU3_MODE,
1045         .name = "IPU3 Pipe Mode",
1046         .type = V4L2_CTRL_TYPE_MENU,
1047         .max = ARRAY_SIZE(imgu_ctrl_mode_strings) - 1,
1048         .def = IPU3_RUNNING_MODE_VIDEO,
1049         .qmenu = imgu_ctrl_mode_strings,
1050 };
1051
1052 /******************** Framework registration ********************/
1053
1054 /* helper function to config node's video properties */
1055 static void imgu_node_to_v4l2(u32 node, struct video_device *vdev,
1056                               struct v4l2_format *f)
1057 {
1058         u32 cap;
1059
1060         /* Should not happen */
1061         WARN_ON(node >= IMGU_NODE_NUM);
1062
1063         switch (node) {
1064         case IMGU_NODE_IN:
1065                 cap = V4L2_CAP_VIDEO_OUTPUT_MPLANE;
1066                 f->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1067                 vdev->ioctl_ops = &imgu_v4l2_ioctl_ops;
1068                 break;
1069         case IMGU_NODE_PARAMS:
1070                 cap = V4L2_CAP_META_OUTPUT;
1071                 f->type = V4L2_BUF_TYPE_META_OUTPUT;
1072                 f->fmt.meta.dataformat = V4L2_META_FMT_IPU3_PARAMS;
1073                 vdev->ioctl_ops = &imgu_v4l2_meta_ioctl_ops;
1074                 imgu_css_meta_fmt_set(&f->fmt.meta);
1075                 break;
1076         case IMGU_NODE_STAT_3A:
1077                 cap = V4L2_CAP_META_CAPTURE;
1078                 f->type = V4L2_BUF_TYPE_META_CAPTURE;
1079                 f->fmt.meta.dataformat = V4L2_META_FMT_IPU3_STAT_3A;
1080                 vdev->ioctl_ops = &imgu_v4l2_meta_ioctl_ops;
1081                 imgu_css_meta_fmt_set(&f->fmt.meta);
1082                 break;
1083         default:
1084                 cap = V4L2_CAP_VIDEO_CAPTURE_MPLANE;
1085                 f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1086                 vdev->ioctl_ops = &imgu_v4l2_ioctl_ops;
1087         }
1088
1089         vdev->device_caps = V4L2_CAP_STREAMING | cap;
1090 }
1091
1092 static int imgu_v4l2_subdev_register(struct imgu_device *imgu,
1093                                      struct imgu_v4l2_subdev *imgu_sd,
1094                                      unsigned int pipe)
1095 {
1096         int i, r;
1097         struct v4l2_ctrl_handler *hdl = &imgu_sd->ctrl_handler;
1098         struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
1099
1100         /* Initialize subdev media entity */
1101         r = media_entity_pads_init(&imgu_sd->subdev.entity, IMGU_NODE_NUM,
1102                                    imgu_sd->subdev_pads);
1103         if (r) {
1104                 dev_err(&imgu->pci_dev->dev,
1105                         "failed initialize subdev media entity (%d)\n", r);
1106                 return r;
1107         }
1108         imgu_sd->subdev.entity.ops = &imgu_media_ops;
1109         for (i = 0; i < IMGU_NODE_NUM; i++) {
1110                 imgu_sd->subdev_pads[i].flags = imgu_pipe->nodes[i].output ?
1111                         MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
1112         }
1113
1114         /* Initialize subdev */
1115         v4l2_subdev_init(&imgu_sd->subdev, &imgu_subdev_ops);
1116         imgu_sd->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_STATISTICS;
1117         imgu_sd->subdev.internal_ops = &imgu_subdev_internal_ops;
1118         imgu_sd->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE |
1119                                 V4L2_SUBDEV_FL_HAS_EVENTS;
1120         snprintf(imgu_sd->subdev.name, sizeof(imgu_sd->subdev.name),
1121                  "%s %u", IMGU_NAME, pipe);
1122         v4l2_set_subdevdata(&imgu_sd->subdev, imgu);
1123         atomic_set(&imgu_sd->running_mode, IPU3_RUNNING_MODE_VIDEO);
1124         v4l2_ctrl_handler_init(hdl, 1);
1125         imgu_sd->subdev.ctrl_handler = hdl;
1126         imgu_sd->ctrl = v4l2_ctrl_new_custom(hdl, &imgu_subdev_ctrl_mode, NULL);
1127         if (hdl->error) {
1128                 r = hdl->error;
1129                 dev_err(&imgu->pci_dev->dev,
1130                         "failed to create subdev v4l2 ctrl with err %d", r);
1131                 goto fail_subdev;
1132         }
1133         r = v4l2_device_register_subdev(&imgu->v4l2_dev, &imgu_sd->subdev);
1134         if (r) {
1135                 dev_err(&imgu->pci_dev->dev,
1136                         "failed initialize subdev (%d)\n", r);
1137                 goto fail_subdev;
1138         }
1139
1140         imgu_sd->pipe = pipe;
1141         return 0;
1142
1143 fail_subdev:
1144         v4l2_ctrl_handler_free(imgu_sd->subdev.ctrl_handler);
1145         media_entity_cleanup(&imgu_sd->subdev.entity);
1146
1147         return r;
1148 }
1149
1150 static int imgu_v4l2_node_setup(struct imgu_device *imgu, unsigned int pipe,
1151                                 int node_num)
1152 {
1153         int r;
1154         u32 flags;
1155         struct v4l2_mbus_framefmt def_bus_fmt = { 0 };
1156         struct v4l2_pix_format_mplane def_pix_fmt = { 0 };
1157         struct device *dev = &imgu->pci_dev->dev;
1158         struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
1159         struct v4l2_subdev *sd = &imgu_pipe->imgu_sd.subdev;
1160         struct imgu_video_device *node = &imgu_pipe->nodes[node_num];
1161         struct video_device *vdev = &node->vdev;
1162         struct vb2_queue *vbq = &node->vbq;
1163
1164         /* Initialize formats to default values */
1165         def_bus_fmt.width = 1920;
1166         def_bus_fmt.height = 1080;
1167         def_bus_fmt.code = MEDIA_BUS_FMT_FIXED;
1168         def_bus_fmt.field = V4L2_FIELD_NONE;
1169         def_bus_fmt.colorspace = V4L2_COLORSPACE_RAW;
1170         def_bus_fmt.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
1171         def_bus_fmt.quantization = V4L2_QUANTIZATION_DEFAULT;
1172         def_bus_fmt.xfer_func = V4L2_XFER_FUNC_DEFAULT;
1173
1174         def_pix_fmt.width = def_bus_fmt.width;
1175         def_pix_fmt.height = def_bus_fmt.height;
1176         def_pix_fmt.field = def_bus_fmt.field;
1177         def_pix_fmt.num_planes = 1;
1178         def_pix_fmt.plane_fmt[0].bytesperline = def_pix_fmt.width * 2;
1179         def_pix_fmt.plane_fmt[0].sizeimage =
1180                 def_pix_fmt.height * def_pix_fmt.plane_fmt[0].bytesperline;
1181         def_pix_fmt.flags = 0;
1182         def_pix_fmt.colorspace = def_bus_fmt.colorspace;
1183         def_pix_fmt.ycbcr_enc = def_bus_fmt.ycbcr_enc;
1184         def_pix_fmt.quantization = def_bus_fmt.quantization;
1185         def_pix_fmt.xfer_func = def_bus_fmt.xfer_func;
1186
1187         /* Initialize miscellaneous variables */
1188         mutex_init(&node->lock);
1189         INIT_LIST_HEAD(&node->buffers);
1190
1191         /* Initialize formats to default values */
1192         node->pad_fmt = def_bus_fmt;
1193         node->id = node_num;
1194         node->pipe = pipe;
1195         imgu_node_to_v4l2(node_num, vdev, &node->vdev_fmt);
1196         if (node->vdev_fmt.type ==
1197             V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE ||
1198             node->vdev_fmt.type ==
1199             V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
1200                 def_pix_fmt.pixelformat = node->output ?
1201                         V4L2_PIX_FMT_IPU3_SGRBG10 :
1202                         V4L2_PIX_FMT_NV12;
1203                 node->vdev_fmt.fmt.pix_mp = def_pix_fmt;
1204         }
1205
1206         /* Initialize media entities */
1207         r = media_entity_pads_init(&vdev->entity, 1, &node->vdev_pad);
1208         if (r) {
1209                 dev_err(dev, "failed initialize media entity (%d)\n", r);
1210                 mutex_destroy(&node->lock);
1211                 return r;
1212         }
1213         node->vdev_pad.flags = node->output ?
1214                 MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
1215         vdev->entity.ops = NULL;
1216
1217         /* Initialize vbq */
1218         vbq->type = node->vdev_fmt.type;
1219         vbq->io_modes = VB2_USERPTR | VB2_MMAP | VB2_DMABUF;
1220         vbq->ops = &imgu_vb2_ops;
1221         vbq->mem_ops = &vb2_dma_sg_memops;
1222         if (imgu->buf_struct_size <= 0)
1223                 imgu->buf_struct_size =
1224                         sizeof(struct imgu_vb2_buffer);
1225         vbq->buf_struct_size = imgu->buf_struct_size;
1226         vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1227         /* can streamon w/o buffers */
1228         vbq->min_buffers_needed = 0;
1229         vbq->drv_priv = imgu;
1230         vbq->lock = &node->lock;
1231         r = vb2_queue_init(vbq);
1232         if (r) {
1233                 dev_err(dev, "failed to initialize video queue (%d)", r);
1234                 media_entity_cleanup(&vdev->entity);
1235                 return r;
1236         }
1237
1238         /* Initialize vdev */
1239         snprintf(vdev->name, sizeof(vdev->name), "%s %u %s",
1240                  IMGU_NAME, pipe, node->name);
1241         vdev->release = video_device_release_empty;
1242         vdev->fops = &imgu_v4l2_fops;
1243         vdev->lock = &node->lock;
1244         vdev->v4l2_dev = &imgu->v4l2_dev;
1245         vdev->queue = &node->vbq;
1246         vdev->vfl_dir = node->output ? VFL_DIR_TX : VFL_DIR_RX;
1247         video_set_drvdata(vdev, imgu);
1248         r = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
1249         if (r) {
1250                 dev_err(dev, "failed to register video device (%d)", r);
1251                 media_entity_cleanup(&vdev->entity);
1252                 return r;
1253         }
1254
1255         /* Create link between video node and the subdev pad */
1256         flags = 0;
1257         if (node->enabled)
1258                 flags |= MEDIA_LNK_FL_ENABLED;
1259         if (node->output) {
1260                 r = media_create_pad_link(&vdev->entity, 0, &sd->entity,
1261                                           node_num, flags);
1262         } else {
1263                 if (node->id == IMGU_NODE_OUT) {
1264                         flags |= MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE;
1265                         node->enabled = true;
1266                 }
1267
1268                 r = media_create_pad_link(&sd->entity, node_num, &vdev->entity,
1269                                           0, flags);
1270         }
1271         if (r) {
1272                 dev_err(dev, "failed to create pad link (%d)", r);
1273                 video_unregister_device(vdev);
1274                 return r;
1275         }
1276
1277         return 0;
1278 }
1279
1280 static void imgu_v4l2_nodes_cleanup_pipe(struct imgu_device *imgu,
1281                                          unsigned int pipe, int node)
1282 {
1283         int i;
1284         struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
1285
1286         for (i = 0; i < node; i++) {
1287                 video_unregister_device(&imgu_pipe->nodes[i].vdev);
1288                 media_entity_cleanup(&imgu_pipe->nodes[i].vdev.entity);
1289                 mutex_destroy(&imgu_pipe->nodes[i].lock);
1290         }
1291 }
1292
1293 static int imgu_v4l2_nodes_setup_pipe(struct imgu_device *imgu, int pipe)
1294 {
1295         int i, r;
1296
1297         for (i = 0; i < IMGU_NODE_NUM; i++) {
1298                 r = imgu_v4l2_node_setup(imgu, pipe, i);
1299                 if (r)
1300                         goto cleanup;
1301         }
1302
1303         return 0;
1304
1305 cleanup:
1306         imgu_v4l2_nodes_cleanup_pipe(imgu, pipe, i);
1307         return r;
1308 }
1309
1310 static void imgu_v4l2_subdev_cleanup(struct imgu_device *imgu, unsigned int i)
1311 {
1312         struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[i];
1313
1314         v4l2_device_unregister_subdev(&imgu_pipe->imgu_sd.subdev);
1315         v4l2_ctrl_handler_free(imgu_pipe->imgu_sd.subdev.ctrl_handler);
1316         media_entity_cleanup(&imgu_pipe->imgu_sd.subdev.entity);
1317 }
1318
1319 static void imgu_v4l2_cleanup_pipes(struct imgu_device *imgu, unsigned int pipe)
1320 {
1321         int i;
1322
1323         for (i = 0; i < pipe; i++) {
1324                 imgu_v4l2_nodes_cleanup_pipe(imgu, i, IMGU_NODE_NUM);
1325                 imgu_v4l2_subdev_cleanup(imgu, i);
1326         }
1327 }
1328
1329 static int imgu_v4l2_register_pipes(struct imgu_device *imgu)
1330 {
1331         struct imgu_media_pipe *imgu_pipe;
1332         int i, r;
1333
1334         for (i = 0; i < IMGU_MAX_PIPE_NUM; i++) {
1335                 imgu_pipe = &imgu->imgu_pipe[i];
1336                 r = imgu_v4l2_subdev_register(imgu, &imgu_pipe->imgu_sd, i);
1337                 if (r) {
1338                         dev_err(&imgu->pci_dev->dev,
1339                                 "failed to register subdev%u ret (%d)\n", i, r);
1340                         goto pipes_cleanup;
1341                 }
1342                 r = imgu_v4l2_nodes_setup_pipe(imgu, i);
1343                 if (r) {
1344                         imgu_v4l2_subdev_cleanup(imgu, i);
1345                         goto pipes_cleanup;
1346                 }
1347         }
1348
1349         return 0;
1350
1351 pipes_cleanup:
1352         imgu_v4l2_cleanup_pipes(imgu, i);
1353         return r;
1354 }
1355
1356 int imgu_v4l2_register(struct imgu_device *imgu)
1357 {
1358         int r;
1359
1360         /* Initialize miscellaneous variables */
1361         imgu->streaming = false;
1362
1363         /* Set up media device */
1364         media_device_pci_init(&imgu->media_dev, imgu->pci_dev, IMGU_NAME);
1365
1366         /* Set up v4l2 device */
1367         imgu->v4l2_dev.mdev = &imgu->media_dev;
1368         imgu->v4l2_dev.ctrl_handler = NULL;
1369         r = v4l2_device_register(&imgu->pci_dev->dev, &imgu->v4l2_dev);
1370         if (r) {
1371                 dev_err(&imgu->pci_dev->dev,
1372                         "failed to register V4L2 device (%d)\n", r);
1373                 goto fail_v4l2_dev;
1374         }
1375
1376         r = imgu_v4l2_register_pipes(imgu);
1377         if (r) {
1378                 dev_err(&imgu->pci_dev->dev,
1379                         "failed to register pipes (%d)\n", r);
1380                 goto fail_v4l2_pipes;
1381         }
1382
1383         r = v4l2_device_register_subdev_nodes(&imgu->v4l2_dev);
1384         if (r) {
1385                 dev_err(&imgu->pci_dev->dev,
1386                         "failed to register subdevs (%d)\n", r);
1387                 goto fail_subdevs;
1388         }
1389
1390         r = media_device_register(&imgu->media_dev);
1391         if (r) {
1392                 dev_err(&imgu->pci_dev->dev,
1393                         "failed to register media device (%d)\n", r);
1394                 goto fail_subdevs;
1395         }
1396
1397         return 0;
1398
1399 fail_subdevs:
1400         imgu_v4l2_cleanup_pipes(imgu, IMGU_MAX_PIPE_NUM);
1401 fail_v4l2_pipes:
1402         v4l2_device_unregister(&imgu->v4l2_dev);
1403 fail_v4l2_dev:
1404         media_device_cleanup(&imgu->media_dev);
1405
1406         return r;
1407 }
1408
1409 int imgu_v4l2_unregister(struct imgu_device *imgu)
1410 {
1411         media_device_unregister(&imgu->media_dev);
1412         imgu_v4l2_cleanup_pipes(imgu, IMGU_MAX_PIPE_NUM);
1413         v4l2_device_unregister(&imgu->v4l2_dev);
1414         media_device_cleanup(&imgu->media_dev);
1415
1416         return 0;
1417 }
1418
1419 void imgu_v4l2_buffer_done(struct vb2_buffer *vb,
1420                            enum vb2_buffer_state state)
1421 {
1422         struct imgu_vb2_buffer *b =
1423                 container_of(vb, struct imgu_vb2_buffer, vbb.vb2_buf);
1424
1425         list_del(&b->list);
1426         vb2_buffer_done(&b->vbb.vb2_buf, state);
1427 }