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