Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0
0002 // Copyright (C) 2018 Intel Corporation
0003 
0004 #include <linux/module.h>
0005 #include <linux/pm_runtime.h>
0006 
0007 #include <media/v4l2-event.h>
0008 #include <media/v4l2-ioctl.h>
0009 
0010 #include "ipu3.h"
0011 #include "ipu3-dmamap.h"
0012 
0013 /******************** v4l2_subdev_ops ********************/
0014 
0015 #define IPU3_RUNNING_MODE_VIDEO     0
0016 #define IPU3_RUNNING_MODE_STILL     1
0017 
0018 static int imgu_subdev_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
0019 {
0020     struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
0021                             struct imgu_v4l2_subdev,
0022                             subdev);
0023     struct imgu_device *imgu = v4l2_get_subdevdata(sd);
0024     struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[imgu_sd->pipe];
0025     struct v4l2_rect try_crop = {
0026         .top = 0,
0027         .left = 0,
0028     };
0029     unsigned int i;
0030 
0031     try_crop.width =
0032         imgu_pipe->nodes[IMGU_NODE_IN].vdev_fmt.fmt.pix_mp.width;
0033     try_crop.height =
0034         imgu_pipe->nodes[IMGU_NODE_IN].vdev_fmt.fmt.pix_mp.height;
0035 
0036     /* Initialize try_fmt */
0037     for (i = 0; i < IMGU_NODE_NUM; i++) {
0038         struct v4l2_mbus_framefmt *try_fmt =
0039             v4l2_subdev_get_try_format(sd, fh->state, i);
0040 
0041         try_fmt->width = try_crop.width;
0042         try_fmt->height = try_crop.height;
0043         try_fmt->code = imgu_pipe->nodes[i].pad_fmt.code;
0044         try_fmt->field = V4L2_FIELD_NONE;
0045     }
0046 
0047     *v4l2_subdev_get_try_crop(sd, fh->state, IMGU_NODE_IN) = try_crop;
0048     *v4l2_subdev_get_try_compose(sd, fh->state, IMGU_NODE_IN) = try_crop;
0049 
0050     return 0;
0051 }
0052 
0053 static int imgu_subdev_s_stream(struct v4l2_subdev *sd, int enable)
0054 {
0055     int i;
0056     unsigned int node;
0057     int r = 0;
0058     struct imgu_device *imgu = v4l2_get_subdevdata(sd);
0059     struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
0060                             struct imgu_v4l2_subdev,
0061                             subdev);
0062     unsigned int pipe = imgu_sd->pipe;
0063     struct device *dev = &imgu->pci_dev->dev;
0064     struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES] = { NULL };
0065     struct v4l2_rect *rects[IPU3_CSS_RECTS] = { NULL };
0066     struct imgu_css_pipe *css_pipe = &imgu->css.pipes[pipe];
0067     struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
0068 
0069     dev_dbg(dev, "%s %d for pipe %u", __func__, enable, pipe);
0070     /* grab ctrl after streamon and return after off */
0071     v4l2_ctrl_grab(imgu_sd->ctrl, enable);
0072 
0073     if (!enable) {
0074         imgu_sd->active = false;
0075         return 0;
0076     }
0077 
0078     for (i = 0; i < IMGU_NODE_NUM; i++)
0079         imgu_pipe->queue_enabled[i] = imgu_pipe->nodes[i].enabled;
0080 
0081     /* This is handled specially */
0082     imgu_pipe->queue_enabled[IPU3_CSS_QUEUE_PARAMS] = false;
0083 
0084     /* Initialize CSS formats */
0085     for (i = 0; i < IPU3_CSS_QUEUES; i++) {
0086         node = imgu_map_node(imgu, i);
0087         /* No need to reconfig meta nodes */
0088         if (node == IMGU_NODE_STAT_3A || node == IMGU_NODE_PARAMS)
0089             continue;
0090         fmts[i] = imgu_pipe->queue_enabled[node] ?
0091             &imgu_pipe->nodes[node].vdev_fmt.fmt.pix_mp : NULL;
0092     }
0093 
0094     /* Enable VF output only when VF queue requested by user */
0095     css_pipe->vf_output_en = false;
0096     if (imgu_pipe->nodes[IMGU_NODE_VF].enabled)
0097         css_pipe->vf_output_en = true;
0098 
0099     if (atomic_read(&imgu_sd->running_mode) == IPU3_RUNNING_MODE_VIDEO)
0100         css_pipe->pipe_id = IPU3_CSS_PIPE_ID_VIDEO;
0101     else
0102         css_pipe->pipe_id = IPU3_CSS_PIPE_ID_CAPTURE;
0103 
0104     dev_dbg(dev, "IPU3 pipe %u pipe_id %u", pipe, css_pipe->pipe_id);
0105 
0106     rects[IPU3_CSS_RECT_EFFECTIVE] = &imgu_sd->rect.eff;
0107     rects[IPU3_CSS_RECT_BDS] = &imgu_sd->rect.bds;
0108     rects[IPU3_CSS_RECT_GDC] = &imgu_sd->rect.gdc;
0109 
0110     r = imgu_css_fmt_set(&imgu->css, fmts, rects, pipe);
0111     if (r) {
0112         dev_err(dev, "failed to set initial formats pipe %u with (%d)",
0113             pipe, r);
0114         return r;
0115     }
0116 
0117     imgu_sd->active = true;
0118 
0119     return 0;
0120 }
0121 
0122 static int imgu_subdev_get_fmt(struct v4l2_subdev *sd,
0123                    struct v4l2_subdev_state *sd_state,
0124                    struct v4l2_subdev_format *fmt)
0125 {
0126     struct imgu_device *imgu = v4l2_get_subdevdata(sd);
0127     struct v4l2_mbus_framefmt *mf;
0128     struct imgu_media_pipe *imgu_pipe;
0129     u32 pad = fmt->pad;
0130     struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
0131                             struct imgu_v4l2_subdev,
0132                             subdev);
0133     unsigned int pipe = imgu_sd->pipe;
0134 
0135     imgu_pipe = &imgu->imgu_pipe[pipe];
0136     if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
0137         fmt->format = imgu_pipe->nodes[pad].pad_fmt;
0138     } else {
0139         mf = v4l2_subdev_get_try_format(sd, sd_state, pad);
0140         fmt->format = *mf;
0141     }
0142 
0143     return 0;
0144 }
0145 
0146 static int imgu_subdev_set_fmt(struct v4l2_subdev *sd,
0147                    struct v4l2_subdev_state *sd_state,
0148                    struct v4l2_subdev_format *fmt)
0149 {
0150     struct imgu_media_pipe *imgu_pipe;
0151     struct imgu_device *imgu = v4l2_get_subdevdata(sd);
0152     struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
0153                             struct imgu_v4l2_subdev,
0154                             subdev);
0155     struct v4l2_mbus_framefmt *mf;
0156     u32 pad = fmt->pad;
0157     unsigned int pipe = imgu_sd->pipe;
0158 
0159     dev_dbg(&imgu->pci_dev->dev, "set subdev %u pad %u fmt to [%ux%u]",
0160         pipe, pad, fmt->format.width, fmt->format.height);
0161 
0162     imgu_pipe = &imgu->imgu_pipe[pipe];
0163     if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
0164         mf = v4l2_subdev_get_try_format(sd, sd_state, pad);
0165     else
0166         mf = &imgu_pipe->nodes[pad].pad_fmt;
0167 
0168     fmt->format.code = mf->code;
0169     /* Clamp the w and h based on the hardware capabilities */
0170     if (imgu_sd->subdev_pads[pad].flags & MEDIA_PAD_FL_SOURCE) {
0171         fmt->format.width = clamp(fmt->format.width,
0172                       IPU3_OUTPUT_MIN_WIDTH,
0173                       IPU3_OUTPUT_MAX_WIDTH);
0174         fmt->format.height = clamp(fmt->format.height,
0175                        IPU3_OUTPUT_MIN_HEIGHT,
0176                        IPU3_OUTPUT_MAX_HEIGHT);
0177     } else {
0178         fmt->format.width = clamp(fmt->format.width,
0179                       IPU3_INPUT_MIN_WIDTH,
0180                       IPU3_INPUT_MAX_WIDTH);
0181         fmt->format.height = clamp(fmt->format.height,
0182                        IPU3_INPUT_MIN_HEIGHT,
0183                        IPU3_INPUT_MAX_HEIGHT);
0184     }
0185 
0186     *mf = fmt->format;
0187 
0188     return 0;
0189 }
0190 
0191 static int imgu_subdev_get_selection(struct v4l2_subdev *sd,
0192                      struct v4l2_subdev_state *sd_state,
0193                      struct v4l2_subdev_selection *sel)
0194 {
0195     struct v4l2_rect *try_sel, *r;
0196     struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
0197                             struct imgu_v4l2_subdev,
0198                             subdev);
0199 
0200     if (sel->pad != IMGU_NODE_IN)
0201         return -EINVAL;
0202 
0203     switch (sel->target) {
0204     case V4L2_SEL_TGT_CROP:
0205         try_sel = v4l2_subdev_get_try_crop(sd, sd_state, sel->pad);
0206         r = &imgu_sd->rect.eff;
0207         break;
0208     case V4L2_SEL_TGT_COMPOSE:
0209         try_sel = v4l2_subdev_get_try_compose(sd, sd_state, sel->pad);
0210         r = &imgu_sd->rect.bds;
0211         break;
0212     default:
0213         return -EINVAL;
0214     }
0215 
0216     if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
0217         sel->r = *try_sel;
0218     else
0219         sel->r = *r;
0220 
0221     return 0;
0222 }
0223 
0224 static int imgu_subdev_set_selection(struct v4l2_subdev *sd,
0225                      struct v4l2_subdev_state *sd_state,
0226                      struct v4l2_subdev_selection *sel)
0227 {
0228     struct imgu_device *imgu = v4l2_get_subdevdata(sd);
0229     struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
0230                             struct imgu_v4l2_subdev,
0231                             subdev);
0232     struct v4l2_rect *rect, *try_sel;
0233 
0234     dev_dbg(&imgu->pci_dev->dev,
0235          "set subdev %u sel which %u target 0x%4x rect [%ux%u]",
0236          imgu_sd->pipe, sel->which, sel->target,
0237          sel->r.width, sel->r.height);
0238 
0239     if (sel->pad != IMGU_NODE_IN)
0240         return -EINVAL;
0241 
0242     switch (sel->target) {
0243     case V4L2_SEL_TGT_CROP:
0244         try_sel = v4l2_subdev_get_try_crop(sd, sd_state, sel->pad);
0245         rect = &imgu_sd->rect.eff;
0246         break;
0247     case V4L2_SEL_TGT_COMPOSE:
0248         try_sel = v4l2_subdev_get_try_compose(sd, sd_state, sel->pad);
0249         rect = &imgu_sd->rect.bds;
0250         break;
0251     default:
0252         return -EINVAL;
0253     }
0254 
0255     if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
0256         *try_sel = sel->r;
0257     else
0258         *rect = sel->r;
0259 
0260     return 0;
0261 }
0262 
0263 /******************** media_entity_operations ********************/
0264 
0265 static int imgu_link_setup(struct media_entity *entity,
0266                const struct media_pad *local,
0267                const struct media_pad *remote, u32 flags)
0268 {
0269     struct imgu_media_pipe *imgu_pipe;
0270     struct v4l2_subdev *sd = container_of(entity, struct v4l2_subdev,
0271                           entity);
0272     struct imgu_device *imgu = v4l2_get_subdevdata(sd);
0273     struct imgu_v4l2_subdev *imgu_sd = container_of(sd,
0274                             struct imgu_v4l2_subdev,
0275                             subdev);
0276     unsigned int pipe = imgu_sd->pipe;
0277     u32 pad = local->index;
0278 
0279     WARN_ON(pad >= IMGU_NODE_NUM);
0280 
0281     dev_dbg(&imgu->pci_dev->dev, "pipe %u pad %u is %s", pipe, pad,
0282          flags & MEDIA_LNK_FL_ENABLED ? "enabled" : "disabled");
0283 
0284     imgu_pipe = &imgu->imgu_pipe[pipe];
0285     imgu_pipe->nodes[pad].enabled = flags & MEDIA_LNK_FL_ENABLED;
0286 
0287     /* enable input node to enable the pipe */
0288     if (pad != IMGU_NODE_IN)
0289         return 0;
0290 
0291     if (flags & MEDIA_LNK_FL_ENABLED)
0292         __set_bit(pipe, imgu->css.enabled_pipes);
0293     else
0294         __clear_bit(pipe, imgu->css.enabled_pipes);
0295 
0296     dev_dbg(&imgu->pci_dev->dev, "pipe %u is %s", pipe,
0297          flags & MEDIA_LNK_FL_ENABLED ? "enabled" : "disabled");
0298 
0299     return 0;
0300 }
0301 
0302 /******************** vb2_ops ********************/
0303 
0304 static int imgu_vb2_buf_init(struct vb2_buffer *vb)
0305 {
0306     struct sg_table *sg = vb2_dma_sg_plane_desc(vb, 0);
0307     struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
0308     struct imgu_buffer *buf = container_of(vb,
0309         struct imgu_buffer, vid_buf.vbb.vb2_buf);
0310     struct imgu_video_device *node =
0311         container_of(vb->vb2_queue, struct imgu_video_device, vbq);
0312     unsigned int queue = imgu_node_to_queue(node->id);
0313 
0314     if (queue == IPU3_CSS_QUEUE_PARAMS)
0315         return 0;
0316 
0317     return imgu_dmamap_map_sg(imgu, sg->sgl, sg->nents, &buf->map);
0318 }
0319 
0320 /* Called when each buffer is freed */
0321 static void imgu_vb2_buf_cleanup(struct vb2_buffer *vb)
0322 {
0323     struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
0324     struct imgu_buffer *buf = container_of(vb,
0325         struct imgu_buffer, vid_buf.vbb.vb2_buf);
0326     struct imgu_video_device *node =
0327         container_of(vb->vb2_queue, struct imgu_video_device, vbq);
0328     unsigned int queue = imgu_node_to_queue(node->id);
0329 
0330     if (queue == IPU3_CSS_QUEUE_PARAMS)
0331         return;
0332 
0333     imgu_dmamap_unmap(imgu, &buf->map);
0334 }
0335 
0336 /* Transfer buffer ownership to me */
0337 static void imgu_vb2_buf_queue(struct vb2_buffer *vb)
0338 {
0339     struct imgu_device *imgu = vb2_get_drv_priv(vb->vb2_queue);
0340     struct imgu_video_device *node =
0341         container_of(vb->vb2_queue, struct imgu_video_device, vbq);
0342     unsigned int queue = imgu_node_to_queue(node->id);
0343     struct imgu_buffer *buf = container_of(vb, struct imgu_buffer,
0344                            vid_buf.vbb.vb2_buf);
0345     unsigned long need_bytes;
0346     unsigned long payload = vb2_get_plane_payload(vb, 0);
0347 
0348     if (vb->vb2_queue->type == V4L2_BUF_TYPE_META_CAPTURE ||
0349         vb->vb2_queue->type == V4L2_BUF_TYPE_META_OUTPUT)
0350         need_bytes = node->vdev_fmt.fmt.meta.buffersize;
0351     else
0352         need_bytes = node->vdev_fmt.fmt.pix_mp.plane_fmt[0].sizeimage;
0353 
0354     if (queue == IPU3_CSS_QUEUE_PARAMS && payload && payload < need_bytes) {
0355         dev_err(&imgu->pci_dev->dev, "invalid data size for params.");
0356         vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
0357         return;
0358     }
0359 
0360     mutex_lock(&imgu->lock);
0361     if (queue != IPU3_CSS_QUEUE_PARAMS)
0362         imgu_css_buf_init(&buf->css_buf, queue, buf->map.daddr);
0363 
0364     list_add_tail(&buf->vid_buf.list, &node->buffers);
0365     mutex_unlock(&imgu->lock);
0366 
0367     vb2_set_plane_payload(vb, 0, need_bytes);
0368 
0369     mutex_lock(&imgu->streaming_lock);
0370     if (imgu->streaming)
0371         imgu_queue_buffers(imgu, false, node->pipe);
0372     mutex_unlock(&imgu->streaming_lock);
0373 
0374     dev_dbg(&imgu->pci_dev->dev, "%s for pipe %u node %u", __func__,
0375         node->pipe, node->id);
0376 }
0377 
0378 static int imgu_vb2_queue_setup(struct vb2_queue *vq,
0379                 unsigned int *num_buffers,
0380                 unsigned int *num_planes,
0381                 unsigned int sizes[],
0382                 struct device *alloc_devs[])
0383 {
0384     struct imgu_device *imgu = vb2_get_drv_priv(vq);
0385     struct imgu_video_device *node =
0386         container_of(vq, struct imgu_video_device, vbq);
0387     const struct v4l2_format *fmt = &node->vdev_fmt;
0388     unsigned int size;
0389 
0390     *num_buffers = clamp_val(*num_buffers, 1, VB2_MAX_FRAME);
0391     alloc_devs[0] = &imgu->pci_dev->dev;
0392 
0393     if (vq->type == V4L2_BUF_TYPE_META_CAPTURE ||
0394         vq->type == V4L2_BUF_TYPE_META_OUTPUT)
0395         size = fmt->fmt.meta.buffersize;
0396     else
0397         size = fmt->fmt.pix_mp.plane_fmt[0].sizeimage;
0398 
0399     if (*num_planes) {
0400         if (sizes[0] < size)
0401             return -EINVAL;
0402         size = sizes[0];
0403     }
0404 
0405     *num_planes = 1;
0406     sizes[0] = size;
0407 
0408     /* Initialize buffer queue */
0409     INIT_LIST_HEAD(&node->buffers);
0410 
0411     return 0;
0412 }
0413 
0414 /* Check if all enabled video nodes are streaming, exception ignored */
0415 static bool imgu_all_nodes_streaming(struct imgu_device *imgu,
0416                      struct imgu_video_device *except)
0417 {
0418     unsigned int i, pipe, p;
0419     struct imgu_video_device *node;
0420     struct device *dev = &imgu->pci_dev->dev;
0421 
0422     pipe = except->pipe;
0423     if (!test_bit(pipe, imgu->css.enabled_pipes)) {
0424         dev_warn(&imgu->pci_dev->dev,
0425              "pipe %u link is not ready yet", pipe);
0426         return false;
0427     }
0428 
0429     for_each_set_bit(p, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
0430         for (i = 0; i < IMGU_NODE_NUM; i++) {
0431             node = &imgu->imgu_pipe[p].nodes[i];
0432             dev_dbg(dev, "%s pipe %u queue %u name %s enabled = %u",
0433                 __func__, p, i, node->name, node->enabled);
0434             if (node == except)
0435                 continue;
0436             if (node->enabled && !vb2_start_streaming_called(&node->vbq))
0437                 return false;
0438         }
0439     }
0440 
0441     return true;
0442 }
0443 
0444 static void imgu_return_all_buffers(struct imgu_device *imgu,
0445                     struct imgu_video_device *node,
0446                     enum vb2_buffer_state state)
0447 {
0448     struct imgu_vb2_buffer *b, *b0;
0449 
0450     /* Return all buffers */
0451     mutex_lock(&imgu->lock);
0452     list_for_each_entry_safe(b, b0, &node->buffers, list) {
0453         list_del(&b->list);
0454         vb2_buffer_done(&b->vbb.vb2_buf, state);
0455     }
0456     mutex_unlock(&imgu->lock);
0457 }
0458 
0459 static int imgu_vb2_start_streaming(struct vb2_queue *vq, unsigned int count)
0460 {
0461     struct imgu_media_pipe *imgu_pipe;
0462     struct imgu_device *imgu = vb2_get_drv_priv(vq);
0463     struct device *dev = &imgu->pci_dev->dev;
0464     struct imgu_video_device *node =
0465         container_of(vq, struct imgu_video_device, vbq);
0466     int r;
0467     unsigned int pipe;
0468 
0469     dev_dbg(dev, "%s node name %s pipe %u id %u", __func__,
0470         node->name, node->pipe, node->id);
0471 
0472     mutex_lock(&imgu->streaming_lock);
0473     if (imgu->streaming) {
0474         r = -EBUSY;
0475         mutex_unlock(&imgu->streaming_lock);
0476         goto fail_return_bufs;
0477     }
0478     mutex_unlock(&imgu->streaming_lock);
0479 
0480     if (!node->enabled) {
0481         dev_err(dev, "IMGU node is not enabled");
0482         r = -EINVAL;
0483         goto fail_return_bufs;
0484     }
0485 
0486     pipe = node->pipe;
0487     imgu_pipe = &imgu->imgu_pipe[pipe];
0488     atomic_set(&node->sequence, 0);
0489     r = media_pipeline_start(&node->vdev.entity, &imgu_pipe->pipeline);
0490     if (r < 0)
0491         goto fail_return_bufs;
0492 
0493     if (!imgu_all_nodes_streaming(imgu, node))
0494         return 0;
0495 
0496     for_each_set_bit(pipe, imgu->css.enabled_pipes, IMGU_MAX_PIPE_NUM) {
0497         r = v4l2_subdev_call(&imgu->imgu_pipe[pipe].imgu_sd.subdev,
0498                      video, s_stream, 1);
0499         if (r < 0)
0500             goto fail_stop_pipeline;
0501     }
0502 
0503     /* Start streaming of the whole pipeline now */
0504     dev_dbg(dev, "IMGU streaming is ready to start");
0505     mutex_lock(&imgu->streaming_lock);
0506     r = imgu_s_stream(imgu, true);
0507     if (!r)
0508         imgu->streaming = true;
0509     mutex_unlock(&imgu->streaming_lock);
0510 
0511     return 0;
0512 
0513 fail_stop_pipeline:
0514     media_pipeline_stop(&node->vdev.entity);
0515 fail_return_bufs:
0516     imgu_return_all_buffers(imgu, node, VB2_BUF_STATE_QUEUED);
0517 
0518     return r;
0519 }
0520 
0521 static void imgu_vb2_stop_streaming(struct vb2_queue *vq)
0522 {
0523     struct imgu_media_pipe *imgu_pipe;
0524     struct imgu_device *imgu = vb2_get_drv_priv(vq);
0525     struct device *dev = &imgu->pci_dev->dev;
0526     struct imgu_video_device *node =
0527         container_of(vq, struct imgu_video_device, vbq);
0528     int r;
0529     unsigned int pipe;
0530 
0531     WARN_ON(!node->enabled);
0532 
0533     pipe = node->pipe;
0534     dev_dbg(dev, "Try to stream off node [%u][%u]", pipe, node->id);
0535     imgu_pipe = &imgu->imgu_pipe[pipe];
0536     r = v4l2_subdev_call(&imgu_pipe->imgu_sd.subdev, video, s_stream, 0);
0537     if (r)
0538         dev_err(&imgu->pci_dev->dev,
0539             "failed to stop subdev streaming\n");
0540 
0541     mutex_lock(&imgu->streaming_lock);
0542     /* Was this the first node with streaming disabled? */
0543     if (imgu->streaming && imgu_all_nodes_streaming(imgu, node)) {
0544         /* Yes, really stop streaming now */
0545         dev_dbg(dev, "IMGU streaming is ready to stop");
0546         r = imgu_s_stream(imgu, false);
0547         if (!r)
0548             imgu->streaming = false;
0549     }
0550 
0551     imgu_return_all_buffers(imgu, node, VB2_BUF_STATE_ERROR);
0552     mutex_unlock(&imgu->streaming_lock);
0553 
0554     media_pipeline_stop(&node->vdev.entity);
0555 }
0556 
0557 /******************** v4l2_ioctl_ops ********************/
0558 
0559 #define VID_CAPTURE 0
0560 #define VID_OUTPUT  1
0561 #define DEF_VID_CAPTURE 0
0562 #define DEF_VID_OUTPUT  1
0563 
0564 struct imgu_fmt {
0565     u32 fourcc;
0566     u16 type; /* VID_CAPTURE or VID_OUTPUT not both */
0567 };
0568 
0569 /* format descriptions for capture and preview */
0570 static const struct imgu_fmt formats[] = {
0571     { V4L2_PIX_FMT_NV12, VID_CAPTURE },
0572     { V4L2_PIX_FMT_IPU3_SGRBG10, VID_OUTPUT },
0573     { V4L2_PIX_FMT_IPU3_SBGGR10, VID_OUTPUT },
0574     { V4L2_PIX_FMT_IPU3_SGBRG10, VID_OUTPUT },
0575     { V4L2_PIX_FMT_IPU3_SRGGB10, VID_OUTPUT },
0576 };
0577 
0578 /* Find the first matched format, return default if not found */
0579 static const struct imgu_fmt *find_format(struct v4l2_format *f, u32 type)
0580 {
0581     unsigned int i;
0582 
0583     for (i = 0; i < ARRAY_SIZE(formats); i++) {
0584         if (formats[i].fourcc == f->fmt.pix_mp.pixelformat &&
0585             formats[i].type == type)
0586             return &formats[i];
0587     }
0588 
0589     return type == VID_CAPTURE ? &formats[DEF_VID_CAPTURE] :
0590                      &formats[DEF_VID_OUTPUT];
0591 }
0592 
0593 static int imgu_vidioc_querycap(struct file *file, void *fh,
0594                 struct v4l2_capability *cap)
0595 {
0596     struct imgu_device *imgu = video_drvdata(file);
0597 
0598     strscpy(cap->driver, IMGU_NAME, sizeof(cap->driver));
0599     strscpy(cap->card, IMGU_NAME, sizeof(cap->card));
0600     snprintf(cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
0601          pci_name(imgu->pci_dev));
0602 
0603     return 0;
0604 }
0605 
0606 static int enum_fmts(struct v4l2_fmtdesc *f, u32 type)
0607 {
0608     unsigned int i, j;
0609 
0610     if (f->mbus_code != 0 && f->mbus_code != MEDIA_BUS_FMT_FIXED)
0611         return -EINVAL;
0612 
0613     for (i = j = 0; i < ARRAY_SIZE(formats); ++i) {
0614         if (formats[i].type == type) {
0615             if (j == f->index)
0616                 break;
0617             ++j;
0618         }
0619     }
0620 
0621     if (i < ARRAY_SIZE(formats)) {
0622         f->pixelformat = formats[i].fourcc;
0623         return 0;
0624     }
0625 
0626     return -EINVAL;
0627 }
0628 
0629 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
0630                    struct v4l2_fmtdesc *f)
0631 {
0632     if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
0633         return -EINVAL;
0634 
0635     return enum_fmts(f, VID_CAPTURE);
0636 }
0637 
0638 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
0639                    struct v4l2_fmtdesc *f)
0640 {
0641     if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
0642         return -EINVAL;
0643 
0644     return enum_fmts(f, VID_OUTPUT);
0645 }
0646 
0647 /* Propagate forward always the format from the CIO2 subdev */
0648 static int imgu_vidioc_g_fmt(struct file *file, void *fh,
0649                  struct v4l2_format *f)
0650 {
0651     struct imgu_video_device *node = file_to_intel_imgu_node(file);
0652 
0653     f->fmt = node->vdev_fmt.fmt;
0654 
0655     return 0;
0656 }
0657 
0658 /*
0659  * Set input/output format. Unless it is just a try, this also resets
0660  * selections (ie. effective and BDS resolutions) to defaults.
0661  */
0662 static int imgu_fmt(struct imgu_device *imgu, unsigned int pipe, int node,
0663             struct v4l2_format *f, bool try)
0664 {
0665     struct device *dev = &imgu->pci_dev->dev;
0666     struct v4l2_pix_format_mplane *fmts[IPU3_CSS_QUEUES] = { NULL };
0667     struct v4l2_rect *rects[IPU3_CSS_RECTS] = { NULL };
0668     struct v4l2_mbus_framefmt pad_fmt;
0669     unsigned int i, css_q;
0670     int ret;
0671     struct imgu_css_pipe *css_pipe = &imgu->css.pipes[pipe];
0672     struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
0673     struct imgu_v4l2_subdev *imgu_sd = &imgu_pipe->imgu_sd;
0674 
0675     dev_dbg(dev, "set fmt node [%u][%u](try = %u)", pipe, node, try);
0676 
0677     for (i = 0; i < IMGU_NODE_NUM; i++)
0678         dev_dbg(dev, "IMGU pipe %u node %u enabled = %u",
0679             pipe, i, imgu_pipe->nodes[i].enabled);
0680 
0681     if (imgu_pipe->nodes[IMGU_NODE_VF].enabled)
0682         css_pipe->vf_output_en = true;
0683 
0684     if (atomic_read(&imgu_sd->running_mode) == IPU3_RUNNING_MODE_VIDEO)
0685         css_pipe->pipe_id = IPU3_CSS_PIPE_ID_VIDEO;
0686     else
0687         css_pipe->pipe_id = IPU3_CSS_PIPE_ID_CAPTURE;
0688 
0689     dev_dbg(dev, "IPU3 pipe %u pipe_id = %u", pipe, css_pipe->pipe_id);
0690 
0691     css_q = imgu_node_to_queue(node);
0692     for (i = 0; i < IPU3_CSS_QUEUES; i++) {
0693         unsigned int inode = imgu_map_node(imgu, i);
0694 
0695         /* Skip the meta node */
0696         if (inode == IMGU_NODE_STAT_3A || inode == IMGU_NODE_PARAMS)
0697             continue;
0698 
0699         /* CSS expects some format on OUT queue */
0700         if (i != IPU3_CSS_QUEUE_OUT &&
0701             !imgu_pipe->nodes[inode].enabled && !try) {
0702             fmts[i] = NULL;
0703             continue;
0704         }
0705 
0706         if (i == css_q) {
0707             fmts[i] = &f->fmt.pix_mp;
0708             continue;
0709         }
0710 
0711         if (try) {
0712             fmts[i] = kmemdup(&imgu_pipe->nodes[inode].vdev_fmt.fmt.pix_mp,
0713                       sizeof(struct v4l2_pix_format_mplane),
0714                       GFP_KERNEL);
0715             if (!fmts[i]) {
0716                 ret = -ENOMEM;
0717                 goto out;
0718             }
0719         } else {
0720             fmts[i] = &imgu_pipe->nodes[inode].vdev_fmt.fmt.pix_mp;
0721         }
0722 
0723     }
0724 
0725     if (!try) {
0726         /* eff and bds res got by imgu_s_sel */
0727         struct imgu_v4l2_subdev *imgu_sd = &imgu_pipe->imgu_sd;
0728 
0729         rects[IPU3_CSS_RECT_EFFECTIVE] = &imgu_sd->rect.eff;
0730         rects[IPU3_CSS_RECT_BDS] = &imgu_sd->rect.bds;
0731         rects[IPU3_CSS_RECT_GDC] = &imgu_sd->rect.gdc;
0732 
0733         /* suppose that pad fmt was set by subdev s_fmt before */
0734         pad_fmt = imgu_pipe->nodes[IMGU_NODE_IN].pad_fmt;
0735         rects[IPU3_CSS_RECT_GDC]->width = pad_fmt.width;
0736         rects[IPU3_CSS_RECT_GDC]->height = pad_fmt.height;
0737     }
0738 
0739     if (!fmts[css_q]) {
0740         ret = -EINVAL;
0741         goto out;
0742     }
0743 
0744     if (try)
0745         ret = imgu_css_fmt_try(&imgu->css, fmts, rects, pipe);
0746     else
0747         ret = imgu_css_fmt_set(&imgu->css, fmts, rects, pipe);
0748 
0749     /* ret is the binary number in the firmware blob */
0750     if (ret < 0)
0751         goto out;
0752 
0753     /*
0754      * imgu doesn't set the node to the value given by user
0755      * before we return success from this function, so set it here.
0756      */
0757     if (!try)
0758         imgu_pipe->nodes[node].vdev_fmt.fmt.pix_mp = f->fmt.pix_mp;
0759 
0760 out:
0761     if (try) {
0762         for (i = 0; i < IPU3_CSS_QUEUES; i++)
0763             if (i != css_q)
0764                 kfree(fmts[i]);
0765     }
0766 
0767     return ret;
0768 }
0769 
0770 static int imgu_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
0771 {
0772     struct v4l2_pix_format_mplane *pixm = &f->fmt.pix_mp;
0773     const struct imgu_fmt *fmt;
0774 
0775     if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
0776         fmt = find_format(f, VID_CAPTURE);
0777     else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
0778         fmt = find_format(f, VID_OUTPUT);
0779     else
0780         return -EINVAL;
0781 
0782     pixm->pixelformat = fmt->fourcc;
0783 
0784     return 0;
0785 }
0786 
0787 static int imgu_vidioc_try_fmt(struct file *file, void *fh,
0788                    struct v4l2_format *f)
0789 {
0790     struct imgu_device *imgu = video_drvdata(file);
0791     struct device *dev = &imgu->pci_dev->dev;
0792     struct imgu_video_device *node = file_to_intel_imgu_node(file);
0793     struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
0794     int r;
0795 
0796     dev_dbg(dev, "%s [%ux%u] for node %u\n", __func__,
0797         pix_mp->width, pix_mp->height, node->id);
0798 
0799     r = imgu_try_fmt(file, fh, f);
0800     if (r)
0801         return r;
0802 
0803     return imgu_fmt(imgu, node->pipe, node->id, f, true);
0804 }
0805 
0806 static int imgu_vidioc_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
0807 {
0808     struct imgu_device *imgu = video_drvdata(file);
0809     struct device *dev = &imgu->pci_dev->dev;
0810     struct imgu_video_device *node = file_to_intel_imgu_node(file);
0811     struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
0812     int r;
0813 
0814     dev_dbg(dev, "%s [%ux%u] for node %u\n", __func__,
0815         pix_mp->width, pix_mp->height, node->id);
0816 
0817     r = imgu_try_fmt(file, fh, f);
0818     if (r)
0819         return r;
0820 
0821     return imgu_fmt(imgu, node->pipe, node->id, f, false);
0822 }
0823 
0824 struct imgu_meta_fmt {
0825     __u32 fourcc;
0826     char *name;
0827 };
0828 
0829 /* From drivers/media/v4l2-core/v4l2-ioctl.c */
0830 static const struct imgu_meta_fmt meta_fmts[] = {
0831     { V4L2_META_FMT_IPU3_PARAMS, "IPU3 processing parameters" },
0832     { V4L2_META_FMT_IPU3_STAT_3A, "IPU3 3A statistics" },
0833 };
0834 
0835 static int imgu_meta_enum_format(struct file *file, void *fh,
0836                  struct v4l2_fmtdesc *fmt)
0837 {
0838     struct imgu_video_device *node = file_to_intel_imgu_node(file);
0839     unsigned int i = fmt->type == V4L2_BUF_TYPE_META_OUTPUT ? 0 : 1;
0840 
0841     /* Each node is dedicated to only one meta format */
0842     if (fmt->index > 0 || fmt->type != node->vbq.type)
0843         return -EINVAL;
0844 
0845     if (fmt->mbus_code != 0 && fmt->mbus_code != MEDIA_BUS_FMT_FIXED)
0846         return -EINVAL;
0847 
0848     strscpy(fmt->description, meta_fmts[i].name, sizeof(fmt->description));
0849     fmt->pixelformat = meta_fmts[i].fourcc;
0850 
0851     return 0;
0852 }
0853 
0854 static int imgu_vidioc_g_meta_fmt(struct file *file, void *fh,
0855                   struct v4l2_format *f)
0856 {
0857     struct imgu_video_device *node = file_to_intel_imgu_node(file);
0858 
0859     if (f->type != node->vbq.type)
0860         return -EINVAL;
0861 
0862     f->fmt = node->vdev_fmt.fmt;
0863 
0864     return 0;
0865 }
0866 
0867 /******************** function pointers ********************/
0868 
0869 static const struct v4l2_subdev_internal_ops imgu_subdev_internal_ops = {
0870     .open = imgu_subdev_open,
0871 };
0872 
0873 static const struct v4l2_subdev_core_ops imgu_subdev_core_ops = {
0874     .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
0875     .unsubscribe_event = v4l2_event_subdev_unsubscribe,
0876 };
0877 
0878 static const struct v4l2_subdev_video_ops imgu_subdev_video_ops = {
0879     .s_stream = imgu_subdev_s_stream,
0880 };
0881 
0882 static const struct v4l2_subdev_pad_ops imgu_subdev_pad_ops = {
0883     .link_validate = v4l2_subdev_link_validate_default,
0884     .get_fmt = imgu_subdev_get_fmt,
0885     .set_fmt = imgu_subdev_set_fmt,
0886     .get_selection = imgu_subdev_get_selection,
0887     .set_selection = imgu_subdev_set_selection,
0888 };
0889 
0890 static const struct v4l2_subdev_ops imgu_subdev_ops = {
0891     .core = &imgu_subdev_core_ops,
0892     .video = &imgu_subdev_video_ops,
0893     .pad = &imgu_subdev_pad_ops,
0894 };
0895 
0896 static const struct media_entity_operations imgu_media_ops = {
0897     .link_setup = imgu_link_setup,
0898     .link_validate = v4l2_subdev_link_validate,
0899 };
0900 
0901 /****************** vb2_ops of the Q ********************/
0902 
0903 static const struct vb2_ops imgu_vb2_ops = {
0904     .buf_init = imgu_vb2_buf_init,
0905     .buf_cleanup = imgu_vb2_buf_cleanup,
0906     .buf_queue = imgu_vb2_buf_queue,
0907     .queue_setup = imgu_vb2_queue_setup,
0908     .start_streaming = imgu_vb2_start_streaming,
0909     .stop_streaming = imgu_vb2_stop_streaming,
0910     .wait_prepare = vb2_ops_wait_prepare,
0911     .wait_finish = vb2_ops_wait_finish,
0912 };
0913 
0914 /****************** v4l2_file_operations *****************/
0915 
0916 static const struct v4l2_file_operations imgu_v4l2_fops = {
0917     .unlocked_ioctl = video_ioctl2,
0918     .open = v4l2_fh_open,
0919     .release = vb2_fop_release,
0920     .poll = vb2_fop_poll,
0921     .mmap = vb2_fop_mmap,
0922 };
0923 
0924 /******************** v4l2_ioctl_ops ********************/
0925 
0926 static const struct v4l2_ioctl_ops imgu_v4l2_ioctl_ops = {
0927     .vidioc_querycap = imgu_vidioc_querycap,
0928 
0929     .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
0930     .vidioc_g_fmt_vid_cap_mplane = imgu_vidioc_g_fmt,
0931     .vidioc_s_fmt_vid_cap_mplane = imgu_vidioc_s_fmt,
0932     .vidioc_try_fmt_vid_cap_mplane = imgu_vidioc_try_fmt,
0933 
0934     .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
0935     .vidioc_g_fmt_vid_out_mplane = imgu_vidioc_g_fmt,
0936     .vidioc_s_fmt_vid_out_mplane = imgu_vidioc_s_fmt,
0937     .vidioc_try_fmt_vid_out_mplane = imgu_vidioc_try_fmt,
0938 
0939     /* buffer queue management */
0940     .vidioc_reqbufs = vb2_ioctl_reqbufs,
0941     .vidioc_create_bufs = vb2_ioctl_create_bufs,
0942     .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
0943     .vidioc_querybuf = vb2_ioctl_querybuf,
0944     .vidioc_qbuf = vb2_ioctl_qbuf,
0945     .vidioc_dqbuf = vb2_ioctl_dqbuf,
0946     .vidioc_streamon = vb2_ioctl_streamon,
0947     .vidioc_streamoff = vb2_ioctl_streamoff,
0948     .vidioc_expbuf = vb2_ioctl_expbuf,
0949 };
0950 
0951 static const struct v4l2_ioctl_ops imgu_v4l2_meta_ioctl_ops = {
0952     .vidioc_querycap = imgu_vidioc_querycap,
0953 
0954     /* meta capture */
0955     .vidioc_enum_fmt_meta_cap = imgu_meta_enum_format,
0956     .vidioc_g_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
0957     .vidioc_s_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
0958     .vidioc_try_fmt_meta_cap = imgu_vidioc_g_meta_fmt,
0959 
0960     /* meta output */
0961     .vidioc_enum_fmt_meta_out = imgu_meta_enum_format,
0962     .vidioc_g_fmt_meta_out = imgu_vidioc_g_meta_fmt,
0963     .vidioc_s_fmt_meta_out = imgu_vidioc_g_meta_fmt,
0964     .vidioc_try_fmt_meta_out = imgu_vidioc_g_meta_fmt,
0965 
0966     .vidioc_reqbufs = vb2_ioctl_reqbufs,
0967     .vidioc_create_bufs = vb2_ioctl_create_bufs,
0968     .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
0969     .vidioc_querybuf = vb2_ioctl_querybuf,
0970     .vidioc_qbuf = vb2_ioctl_qbuf,
0971     .vidioc_dqbuf = vb2_ioctl_dqbuf,
0972     .vidioc_streamon = vb2_ioctl_streamon,
0973     .vidioc_streamoff = vb2_ioctl_streamoff,
0974     .vidioc_expbuf = vb2_ioctl_expbuf,
0975 };
0976 
0977 static int imgu_sd_s_ctrl(struct v4l2_ctrl *ctrl)
0978 {
0979     struct imgu_v4l2_subdev *imgu_sd =
0980         container_of(ctrl->handler, struct imgu_v4l2_subdev, ctrl_handler);
0981     struct imgu_device *imgu = v4l2_get_subdevdata(&imgu_sd->subdev);
0982     struct device *dev = &imgu->pci_dev->dev;
0983 
0984     dev_dbg(dev, "set val %d to ctrl 0x%8x for subdev %u",
0985         ctrl->val, ctrl->id, imgu_sd->pipe);
0986 
0987     switch (ctrl->id) {
0988     case V4L2_CID_INTEL_IPU3_MODE:
0989         atomic_set(&imgu_sd->running_mode, ctrl->val);
0990         return 0;
0991     default:
0992         return -EINVAL;
0993     }
0994 }
0995 
0996 static const struct v4l2_ctrl_ops imgu_subdev_ctrl_ops = {
0997     .s_ctrl = imgu_sd_s_ctrl,
0998 };
0999 
1000 static const char * const imgu_ctrl_mode_strings[] = {
1001     "Video mode",
1002     "Still mode",
1003 };
1004 
1005 static const struct v4l2_ctrl_config imgu_subdev_ctrl_mode = {
1006     .ops = &imgu_subdev_ctrl_ops,
1007     .id = V4L2_CID_INTEL_IPU3_MODE,
1008     .name = "IPU3 Pipe Mode",
1009     .type = V4L2_CTRL_TYPE_MENU,
1010     .max = ARRAY_SIZE(imgu_ctrl_mode_strings) - 1,
1011     .def = IPU3_RUNNING_MODE_VIDEO,
1012     .qmenu = imgu_ctrl_mode_strings,
1013 };
1014 
1015 /******************** Framework registration ********************/
1016 
1017 /* helper function to config node's video properties */
1018 static void imgu_node_to_v4l2(u32 node, struct video_device *vdev,
1019                   struct v4l2_format *f)
1020 {
1021     u32 cap;
1022 
1023     /* Should not happen */
1024     WARN_ON(node >= IMGU_NODE_NUM);
1025 
1026     switch (node) {
1027     case IMGU_NODE_IN:
1028         cap = V4L2_CAP_VIDEO_OUTPUT_MPLANE;
1029         f->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1030         vdev->ioctl_ops = &imgu_v4l2_ioctl_ops;
1031         break;
1032     case IMGU_NODE_PARAMS:
1033         cap = V4L2_CAP_META_OUTPUT;
1034         f->type = V4L2_BUF_TYPE_META_OUTPUT;
1035         f->fmt.meta.dataformat = V4L2_META_FMT_IPU3_PARAMS;
1036         vdev->ioctl_ops = &imgu_v4l2_meta_ioctl_ops;
1037         imgu_css_meta_fmt_set(&f->fmt.meta);
1038         break;
1039     case IMGU_NODE_STAT_3A:
1040         cap = V4L2_CAP_META_CAPTURE;
1041         f->type = V4L2_BUF_TYPE_META_CAPTURE;
1042         f->fmt.meta.dataformat = V4L2_META_FMT_IPU3_STAT_3A;
1043         vdev->ioctl_ops = &imgu_v4l2_meta_ioctl_ops;
1044         imgu_css_meta_fmt_set(&f->fmt.meta);
1045         break;
1046     default:
1047         cap = V4L2_CAP_VIDEO_CAPTURE_MPLANE;
1048         f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1049         vdev->ioctl_ops = &imgu_v4l2_ioctl_ops;
1050     }
1051 
1052     vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_IO_MC | cap;
1053 }
1054 
1055 static int imgu_v4l2_subdev_register(struct imgu_device *imgu,
1056                      struct imgu_v4l2_subdev *imgu_sd,
1057                      unsigned int pipe)
1058 {
1059     int i, r;
1060     struct v4l2_ctrl_handler *hdl = &imgu_sd->ctrl_handler;
1061     struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
1062 
1063     /* Initialize subdev media entity */
1064     r = media_entity_pads_init(&imgu_sd->subdev.entity, IMGU_NODE_NUM,
1065                    imgu_sd->subdev_pads);
1066     if (r) {
1067         dev_err(&imgu->pci_dev->dev,
1068             "failed initialize subdev media entity (%d)\n", r);
1069         return r;
1070     }
1071     imgu_sd->subdev.entity.ops = &imgu_media_ops;
1072     for (i = 0; i < IMGU_NODE_NUM; i++) {
1073         imgu_sd->subdev_pads[i].flags = imgu_pipe->nodes[i].output ?
1074             MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
1075     }
1076 
1077     /* Initialize subdev */
1078     v4l2_subdev_init(&imgu_sd->subdev, &imgu_subdev_ops);
1079     imgu_sd->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_STATISTICS;
1080     imgu_sd->subdev.internal_ops = &imgu_subdev_internal_ops;
1081     imgu_sd->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE |
1082                 V4L2_SUBDEV_FL_HAS_EVENTS;
1083     snprintf(imgu_sd->subdev.name, sizeof(imgu_sd->subdev.name),
1084          "%s %u", IMGU_NAME, pipe);
1085     v4l2_set_subdevdata(&imgu_sd->subdev, imgu);
1086     atomic_set(&imgu_sd->running_mode, IPU3_RUNNING_MODE_VIDEO);
1087     v4l2_ctrl_handler_init(hdl, 1);
1088     imgu_sd->subdev.ctrl_handler = hdl;
1089     imgu_sd->ctrl = v4l2_ctrl_new_custom(hdl, &imgu_subdev_ctrl_mode, NULL);
1090     if (hdl->error) {
1091         r = hdl->error;
1092         dev_err(&imgu->pci_dev->dev,
1093             "failed to create subdev v4l2 ctrl with err %d", r);
1094         goto fail_subdev;
1095     }
1096     r = v4l2_device_register_subdev(&imgu->v4l2_dev, &imgu_sd->subdev);
1097     if (r) {
1098         dev_err(&imgu->pci_dev->dev,
1099             "failed initialize subdev (%d)\n", r);
1100         goto fail_subdev;
1101     }
1102 
1103     imgu_sd->pipe = pipe;
1104     return 0;
1105 
1106 fail_subdev:
1107     v4l2_ctrl_handler_free(imgu_sd->subdev.ctrl_handler);
1108     media_entity_cleanup(&imgu_sd->subdev.entity);
1109 
1110     return r;
1111 }
1112 
1113 static int imgu_v4l2_node_setup(struct imgu_device *imgu, unsigned int pipe,
1114                 int node_num)
1115 {
1116     int r;
1117     u32 flags;
1118     struct v4l2_mbus_framefmt def_bus_fmt = { 0 };
1119     struct v4l2_pix_format_mplane def_pix_fmt = { 0 };
1120     struct device *dev = &imgu->pci_dev->dev;
1121     struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
1122     struct v4l2_subdev *sd = &imgu_pipe->imgu_sd.subdev;
1123     struct imgu_video_device *node = &imgu_pipe->nodes[node_num];
1124     struct video_device *vdev = &node->vdev;
1125     struct vb2_queue *vbq = &node->vbq;
1126 
1127     /* Initialize formats to default values */
1128     def_bus_fmt.width = 1920;
1129     def_bus_fmt.height = 1080;
1130     def_bus_fmt.code = MEDIA_BUS_FMT_FIXED;
1131     def_bus_fmt.field = V4L2_FIELD_NONE;
1132     def_bus_fmt.colorspace = V4L2_COLORSPACE_RAW;
1133     def_bus_fmt.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
1134     def_bus_fmt.quantization = V4L2_QUANTIZATION_DEFAULT;
1135     def_bus_fmt.xfer_func = V4L2_XFER_FUNC_DEFAULT;
1136 
1137     def_pix_fmt.width = def_bus_fmt.width;
1138     def_pix_fmt.height = def_bus_fmt.height;
1139     def_pix_fmt.field = def_bus_fmt.field;
1140     def_pix_fmt.num_planes = 1;
1141     def_pix_fmt.plane_fmt[0].bytesperline =
1142         imgu_bytesperline(def_pix_fmt.width,
1143                   IMGU_ABI_FRAME_FORMAT_RAW_PACKED);
1144     def_pix_fmt.plane_fmt[0].sizeimage =
1145         def_pix_fmt.height * def_pix_fmt.plane_fmt[0].bytesperline;
1146     def_pix_fmt.flags = 0;
1147     def_pix_fmt.colorspace = def_bus_fmt.colorspace;
1148     def_pix_fmt.ycbcr_enc = def_bus_fmt.ycbcr_enc;
1149     def_pix_fmt.quantization = def_bus_fmt.quantization;
1150     def_pix_fmt.xfer_func = def_bus_fmt.xfer_func;
1151 
1152     /* Initialize miscellaneous variables */
1153     mutex_init(&node->lock);
1154     INIT_LIST_HEAD(&node->buffers);
1155 
1156     /* Initialize formats to default values */
1157     node->pad_fmt = def_bus_fmt;
1158     node->id = node_num;
1159     node->pipe = pipe;
1160     imgu_node_to_v4l2(node_num, vdev, &node->vdev_fmt);
1161     if (node->vdev_fmt.type ==
1162         V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE ||
1163         node->vdev_fmt.type ==
1164         V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
1165         def_pix_fmt.pixelformat = node->output ?
1166             V4L2_PIX_FMT_IPU3_SGRBG10 :
1167             V4L2_PIX_FMT_NV12;
1168         node->vdev_fmt.fmt.pix_mp = def_pix_fmt;
1169     }
1170 
1171     /* Initialize media entities */
1172     r = media_entity_pads_init(&vdev->entity, 1, &node->vdev_pad);
1173     if (r) {
1174         dev_err(dev, "failed initialize media entity (%d)\n", r);
1175         mutex_destroy(&node->lock);
1176         return r;
1177     }
1178     node->vdev_pad.flags = node->output ?
1179         MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
1180     vdev->entity.ops = NULL;
1181 
1182     /* Initialize vbq */
1183     vbq->type = node->vdev_fmt.type;
1184     vbq->io_modes = VB2_USERPTR | VB2_MMAP | VB2_DMABUF;
1185     vbq->ops = &imgu_vb2_ops;
1186     vbq->mem_ops = &vb2_dma_sg_memops;
1187     if (imgu->buf_struct_size <= 0)
1188         imgu->buf_struct_size =
1189             sizeof(struct imgu_vb2_buffer);
1190     vbq->buf_struct_size = imgu->buf_struct_size;
1191     vbq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1192     /* can streamon w/o buffers */
1193     vbq->min_buffers_needed = 0;
1194     vbq->drv_priv = imgu;
1195     vbq->lock = &node->lock;
1196     r = vb2_queue_init(vbq);
1197     if (r) {
1198         dev_err(dev, "failed to initialize video queue (%d)", r);
1199         media_entity_cleanup(&vdev->entity);
1200         return r;
1201     }
1202 
1203     /* Initialize vdev */
1204     snprintf(vdev->name, sizeof(vdev->name), "%s %u %s",
1205          IMGU_NAME, pipe, node->name);
1206     vdev->release = video_device_release_empty;
1207     vdev->fops = &imgu_v4l2_fops;
1208     vdev->lock = &node->lock;
1209     vdev->v4l2_dev = &imgu->v4l2_dev;
1210     vdev->queue = &node->vbq;
1211     vdev->vfl_dir = node->output ? VFL_DIR_TX : VFL_DIR_RX;
1212     video_set_drvdata(vdev, imgu);
1213     r = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1214     if (r) {
1215         dev_err(dev, "failed to register video device (%d)", r);
1216         media_entity_cleanup(&vdev->entity);
1217         return r;
1218     }
1219 
1220     /* Create link between video node and the subdev pad */
1221     flags = 0;
1222     if (node->enabled)
1223         flags |= MEDIA_LNK_FL_ENABLED;
1224     if (node->output) {
1225         r = media_create_pad_link(&vdev->entity, 0, &sd->entity,
1226                       node_num, flags);
1227     } else {
1228         if (node->id == IMGU_NODE_OUT) {
1229             flags |= MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE;
1230             node->enabled = true;
1231         }
1232 
1233         r = media_create_pad_link(&sd->entity, node_num, &vdev->entity,
1234                       0, flags);
1235     }
1236     if (r) {
1237         dev_err(dev, "failed to create pad link (%d)", r);
1238         video_unregister_device(vdev);
1239         return r;
1240     }
1241 
1242     return 0;
1243 }
1244 
1245 static void imgu_v4l2_nodes_cleanup_pipe(struct imgu_device *imgu,
1246                      unsigned int pipe, int node)
1247 {
1248     int i;
1249     struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[pipe];
1250 
1251     for (i = 0; i < node; i++) {
1252         video_unregister_device(&imgu_pipe->nodes[i].vdev);
1253         media_entity_cleanup(&imgu_pipe->nodes[i].vdev.entity);
1254         mutex_destroy(&imgu_pipe->nodes[i].lock);
1255     }
1256 }
1257 
1258 static int imgu_v4l2_nodes_setup_pipe(struct imgu_device *imgu, int pipe)
1259 {
1260     int i;
1261 
1262     for (i = 0; i < IMGU_NODE_NUM; i++) {
1263         int r = imgu_v4l2_node_setup(imgu, pipe, i);
1264 
1265         if (r) {
1266             imgu_v4l2_nodes_cleanup_pipe(imgu, pipe, i);
1267             return r;
1268         }
1269     }
1270     return 0;
1271 }
1272 
1273 static void imgu_v4l2_subdev_cleanup(struct imgu_device *imgu, unsigned int i)
1274 {
1275     struct imgu_media_pipe *imgu_pipe = &imgu->imgu_pipe[i];
1276 
1277     v4l2_device_unregister_subdev(&imgu_pipe->imgu_sd.subdev);
1278     v4l2_ctrl_handler_free(imgu_pipe->imgu_sd.subdev.ctrl_handler);
1279     media_entity_cleanup(&imgu_pipe->imgu_sd.subdev.entity);
1280 }
1281 
1282 static void imgu_v4l2_cleanup_pipes(struct imgu_device *imgu, unsigned int pipe)
1283 {
1284     int i;
1285 
1286     for (i = 0; i < pipe; i++) {
1287         imgu_v4l2_nodes_cleanup_pipe(imgu, i, IMGU_NODE_NUM);
1288         imgu_v4l2_subdev_cleanup(imgu, i);
1289     }
1290 }
1291 
1292 static int imgu_v4l2_register_pipes(struct imgu_device *imgu)
1293 {
1294     struct imgu_media_pipe *imgu_pipe;
1295     int i, r;
1296 
1297     for (i = 0; i < IMGU_MAX_PIPE_NUM; i++) {
1298         imgu_pipe = &imgu->imgu_pipe[i];
1299         r = imgu_v4l2_subdev_register(imgu, &imgu_pipe->imgu_sd, i);
1300         if (r) {
1301             dev_err(&imgu->pci_dev->dev,
1302                 "failed to register subdev%u ret (%d)\n", i, r);
1303             goto pipes_cleanup;
1304         }
1305         r = imgu_v4l2_nodes_setup_pipe(imgu, i);
1306         if (r) {
1307             imgu_v4l2_subdev_cleanup(imgu, i);
1308             goto pipes_cleanup;
1309         }
1310     }
1311 
1312     return 0;
1313 
1314 pipes_cleanup:
1315     imgu_v4l2_cleanup_pipes(imgu, i);
1316     return r;
1317 }
1318 
1319 int imgu_v4l2_register(struct imgu_device *imgu)
1320 {
1321     int r;
1322 
1323     /* Initialize miscellaneous variables */
1324     imgu->streaming = false;
1325 
1326     /* Set up media device */
1327     media_device_pci_init(&imgu->media_dev, imgu->pci_dev, IMGU_NAME);
1328 
1329     /* Set up v4l2 device */
1330     imgu->v4l2_dev.mdev = &imgu->media_dev;
1331     imgu->v4l2_dev.ctrl_handler = NULL;
1332     r = v4l2_device_register(&imgu->pci_dev->dev, &imgu->v4l2_dev);
1333     if (r) {
1334         dev_err(&imgu->pci_dev->dev,
1335             "failed to register V4L2 device (%d)\n", r);
1336         goto fail_v4l2_dev;
1337     }
1338 
1339     r = imgu_v4l2_register_pipes(imgu);
1340     if (r) {
1341         dev_err(&imgu->pci_dev->dev,
1342             "failed to register pipes (%d)\n", r);
1343         goto fail_v4l2_pipes;
1344     }
1345 
1346     r = v4l2_device_register_subdev_nodes(&imgu->v4l2_dev);
1347     if (r) {
1348         dev_err(&imgu->pci_dev->dev,
1349             "failed to register subdevs (%d)\n", r);
1350         goto fail_subdevs;
1351     }
1352 
1353     r = media_device_register(&imgu->media_dev);
1354     if (r) {
1355         dev_err(&imgu->pci_dev->dev,
1356             "failed to register media device (%d)\n", r);
1357         goto fail_subdevs;
1358     }
1359 
1360     return 0;
1361 
1362 fail_subdevs:
1363     imgu_v4l2_cleanup_pipes(imgu, IMGU_MAX_PIPE_NUM);
1364 fail_v4l2_pipes:
1365     v4l2_device_unregister(&imgu->v4l2_dev);
1366 fail_v4l2_dev:
1367     media_device_cleanup(&imgu->media_dev);
1368 
1369     return r;
1370 }
1371 
1372 int imgu_v4l2_unregister(struct imgu_device *imgu)
1373 {
1374     media_device_unregister(&imgu->media_dev);
1375     imgu_v4l2_cleanup_pipes(imgu, IMGU_MAX_PIPE_NUM);
1376     v4l2_device_unregister(&imgu->v4l2_dev);
1377     media_device_cleanup(&imgu->media_dev);
1378 
1379     return 0;
1380 }
1381 
1382 void imgu_v4l2_buffer_done(struct vb2_buffer *vb,
1383                enum vb2_buffer_state state)
1384 {
1385     struct imgu_vb2_buffer *b =
1386         container_of(vb, struct imgu_vb2_buffer, vbb.vb2_buf);
1387 
1388     list_del(&b->list);
1389     vb2_buffer_done(&b->vbb.vb2_buf, state);
1390 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!