media/imx/imx-media-dev-common.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * V4L2 Media Controller Driver for Freescale common i.MX5/6/7 SOC
0004  *
0005  * Copyright (c) 2019 Linaro Ltd
0006  * Copyright (c) 2016 Mentor Graphics Inc.
0007  */
0008
0009 #include <linux/of_graph.h>
0010 #include <linux/of_platform.h>
0011 #include <media/v4l2-ctrls.h>
0012 #include <media/v4l2-event.h>
0013 #include <media/v4l2-ioctl.h>
0014 #include <media/v4l2-mc.h>
0015 #include "imx-media.h"
0016
0017 static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n)
0018 {
0019     return container_of(n, struct imx_media_dev, notifier);
0020 }
0021
0022 /* async subdev bound notifier */
0023 static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier,
0024                   struct v4l2_subdev *sd,
0025                   struct v4l2_async_subdev *asd)
0026 {
0027     struct imx_media_dev *imxmd = notifier2dev(notifier);
0028
0029     dev_dbg(imxmd->md.dev, "subdev %s bound\n", sd->name);
0030
0031     return 0;
0032 }
0033
0034 /*
0035  * Create the missing media links from the CSI-2 receiver.
0036  * Called after all async subdevs have bound.
0037  */
0038 static void imx_media_create_csi2_links(struct imx_media_dev *imxmd)
0039 {
0040     struct v4l2_subdev *sd, *csi2 = NULL;
0041
0042     list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
0043         if (sd->grp_id == IMX_MEDIA_GRP_ID_CSI2) {
0044             csi2 = sd;
0045             break;
0046         }
0047     }
0048     if (!csi2)
0049         return;
0050
0051     list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
0052         /* skip if not a CSI or a CSI mux */
0053         if (!(sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) &&
0054             !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI) &&
0055             !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI_MUX))
0056             continue;
0057
0058         v4l2_create_fwnode_links(csi2, sd);
0059     }
0060 }
0061
0062 /*
0063  * adds given video device to given imx-media source pad vdev list.
0064  * Continues upstream from the pad entity's sink pads.
0065  */
0066 static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd,
0067                      struct imx_media_video_dev *vdev,
0068                      struct media_pad *srcpad)
0069 {
0070     struct media_entity *entity = srcpad->entity;
0071     struct imx_media_pad_vdev *pad_vdev;
0072     struct list_head *pad_vdev_list;
0073     struct media_link *link;
0074     struct v4l2_subdev *sd;
0075     int i, ret;
0076
0077     /* skip this entity if not a v4l2_subdev */
0078     if (!is_media_entity_v4l2_subdev(entity))
0079         return 0;
0080
0081     sd = media_entity_to_v4l2_subdev(entity);
0082
0083     pad_vdev_list = to_pad_vdev_list(sd, srcpad->index);
0084     if (!pad_vdev_list) {
0085         v4l2_warn(&imxmd->v4l2_dev, "%s:%u has no vdev list!\n",
0086               entity->name, srcpad->index);
0087         /*
0088          * shouldn't happen, but no reason to fail driver load,
0089          * just skip this entity.
0090          */
0091         return 0;
0092     }
0093
0094     /* just return if we've been here before */
0095     list_for_each_entry(pad_vdev, pad_vdev_list, list) {
0096         if (pad_vdev->vdev == vdev)
0097             return 0;
0098     }
0099
0100     dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n",
0101         vdev->vfd->entity.name, entity->name, srcpad->index);
0102
0103     pad_vdev = devm_kzalloc(imxmd->md.dev, sizeof(*pad_vdev), GFP_KERNEL);
0104     if (!pad_vdev)
0105         return -ENOMEM;
0106
0107     /* attach this vdev to this pad */
0108     pad_vdev->vdev = vdev;
0109     list_add_tail(&pad_vdev->list, pad_vdev_list);
0110
0111     /* move upstream from this entity's sink pads */
0112     for (i = 0; i < entity->num_pads; i++) {
0113         struct media_pad *pad = &entity->pads[i];
0114
0115         if (!(pad->flags & MEDIA_PAD_FL_SINK))
0116             continue;
0117
0118         list_for_each_entry(link, &entity->links, list) {
0119             if (link->sink != pad)
0120                 continue;
0121             ret = imx_media_add_vdev_to_pad(imxmd, vdev,
0122                             link->source);
0123             if (ret)
0124                 return ret;
0125         }
0126     }
0127
0128     return 0;
0129 }
0130
0131 /*
0132  * For every subdevice, allocate an array of list_head's, one list_head
0133  * for each pad, to hold the list of video devices reachable from that
0134  * pad.
0135  */
0136 static int imx_media_alloc_pad_vdev_lists(struct imx_media_dev *imxmd)
0137 {
0138     struct list_head *vdev_lists;
0139     struct media_entity *entity;
0140     struct v4l2_subdev *sd;
0141     int i;
0142
0143     list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
0144         entity = &sd->entity;
0145         vdev_lists = devm_kcalloc(imxmd->md.dev,
0146                       entity->num_pads, sizeof(*vdev_lists),
0147                       GFP_KERNEL);
0148         if (!vdev_lists)
0149             return -ENOMEM;
0150
0151         /* attach to the subdev's host private pointer */
0152         sd->host_priv = vdev_lists;
0153
0154         for (i = 0; i < entity->num_pads; i++)
0155             INIT_LIST_HEAD(to_pad_vdev_list(sd, i));
0156     }
0157
0158     return 0;
0159 }
0160
0161 /* form the vdev lists in all imx-media source pads */
0162 static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd)
0163 {
0164     struct imx_media_video_dev *vdev;
0165     struct media_link *link;
0166     int ret;
0167
0168     ret = imx_media_alloc_pad_vdev_lists(imxmd);
0169     if (ret)
0170         return ret;
0171
0172     list_for_each_entry(vdev, &imxmd->vdev_list, list) {
0173         link = list_first_entry(&vdev->vfd->entity.links,
0174                     struct media_link, list);
0175         ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source);
0176         if (ret)
0177             return ret;
0178     }
0179
0180     return 0;
0181 }
0182
0183 /* async subdev complete notifier */
0184 int imx_media_probe_complete(struct v4l2_async_notifier *notifier)
0185 {
0186     struct imx_media_dev *imxmd = notifier2dev(notifier);
0187     int ret;
0188
0189     mutex_lock(&imxmd->mutex);
0190
0191     imx_media_create_csi2_links(imxmd);
0192
0193     ret = imx_media_create_pad_vdev_lists(imxmd);
0194     if (ret)
0195         goto unlock;
0196
0197     ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev);
0198 unlock:
0199     mutex_unlock(&imxmd->mutex);
0200     if (ret)
0201         return ret;
0202
0203     return media_device_register(&imxmd->md);
0204 }
0205 EXPORT_SYMBOL_GPL(imx_media_probe_complete);
0206
0207 /*
0208  * adds controls to a video device from an entity subdevice.
0209  * Continues upstream from the entity's sink pads.
0210  */
0211 static int imx_media_inherit_controls(struct imx_media_dev *imxmd,
0212                       struct video_device *vfd,
0213                       struct media_entity *entity)
0214 {
0215     int i, ret = 0;
0216
0217     if (is_media_entity_v4l2_subdev(entity)) {
0218         struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
0219
0220         dev_dbg(imxmd->md.dev,
0221             "adding controls to %s from %s\n",
0222             vfd->entity.name, sd->entity.name);
0223
0224         ret = v4l2_ctrl_add_handler(vfd->ctrl_handler,
0225                         sd->ctrl_handler,
0226                         NULL, true);
0227         if (ret)
0228             return ret;
0229     }
0230
0231     /* move upstream */
0232     for (i = 0; i < entity->num_pads; i++) {
0233         struct media_pad *pad, *spad = &entity->pads[i];
0234
0235         if (!(spad->flags & MEDIA_PAD_FL_SINK))
0236             continue;
0237
0238         pad = media_pad_remote_pad_first(spad);
0239         if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
0240             continue;
0241
0242         ret = imx_media_inherit_controls(imxmd, vfd, pad->entity);
0243         if (ret)
0244             break;
0245     }
0246
0247     return ret;
0248 }
0249
0250 static int imx_media_link_notify(struct media_link *link, u32 flags,
0251                  unsigned int notification)
0252 {
0253     struct imx_media_dev *imxmd = container_of(link->graph_obj.mdev,
0254                            struct imx_media_dev, md);
0255     struct media_entity *source = link->source->entity;
0256     struct imx_media_pad_vdev *pad_vdev;
0257     struct list_head *pad_vdev_list;
0258     struct video_device *vfd;
0259     struct v4l2_subdev *sd;
0260     int pad_idx, ret;
0261
0262     ret = v4l2_pipeline_link_notify(link, flags, notification);
0263     if (ret)
0264         return ret;
0265
0266     /* don't bother if source is not a subdev */
0267     if (!is_media_entity_v4l2_subdev(source))
0268         return 0;
0269
0270     sd = media_entity_to_v4l2_subdev(source);
0271     pad_idx = link->source->index;
0272
0273     pad_vdev_list = to_pad_vdev_list(sd, pad_idx);
0274     if (!pad_vdev_list) {
0275         /* nothing to do if source sd has no pad vdev list */
0276         return 0;
0277     }
0278
0279     /*
0280      * Before disabling a link, reset controls for all video
0281      * devices reachable from this link.
0282      *
0283      * After enabling a link, refresh controls for all video
0284      * devices reachable from this link.
0285      */
0286     if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
0287         !(flags & MEDIA_LNK_FL_ENABLED)) {
0288         list_for_each_entry(pad_vdev, pad_vdev_list, list) {
0289             vfd = pad_vdev->vdev->vfd;
0290             if (!vfd->ctrl_handler)
0291                 continue;
0292             dev_dbg(imxmd->md.dev,
0293                 "reset controls for %s\n",
0294                 vfd->entity.name);
0295             v4l2_ctrl_handler_free(vfd->ctrl_handler);
0296             v4l2_ctrl_handler_init(vfd->ctrl_handler, 0);
0297         }
0298     } else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
0299            (link->flags & MEDIA_LNK_FL_ENABLED)) {
0300         list_for_each_entry(pad_vdev, pad_vdev_list, list) {
0301             vfd = pad_vdev->vdev->vfd;
0302             if (!vfd->ctrl_handler)
0303                 continue;
0304             dev_dbg(imxmd->md.dev,
0305                 "refresh controls for %s\n",
0306                 vfd->entity.name);
0307             ret = imx_media_inherit_controls(imxmd, vfd,
0308                              &vfd->entity);
0309             if (ret)
0310                 break;
0311         }
0312     }
0313
0314     return ret;
0315 }
0316
0317 static void imx_media_notify(struct v4l2_subdev *sd, unsigned int notification,
0318                  void *arg)
0319 {
0320     struct media_entity *entity = &sd->entity;
0321     int i;
0322
0323     if (notification != V4L2_DEVICE_NOTIFY_EVENT)
0324         return;
0325
0326     for (i = 0; i < entity->num_pads; i++) {
0327         struct media_pad *pad = &entity->pads[i];
0328         struct imx_media_pad_vdev *pad_vdev;
0329         struct list_head *pad_vdev_list;
0330
0331         pad_vdev_list = to_pad_vdev_list(sd, pad->index);
0332         if (!pad_vdev_list)
0333             continue;
0334         list_for_each_entry(pad_vdev, pad_vdev_list, list)
0335             v4l2_event_queue(pad_vdev->vdev->vfd, arg);
0336     }
0337 }
0338
0339 static const struct v4l2_async_notifier_operations imx_media_notifier_ops = {
0340     .bound = imx_media_subdev_bound,
0341     .complete = imx_media_probe_complete,
0342 };
0343
0344 static const struct media_device_ops imx_media_md_ops = {
0345     .link_notify = imx_media_link_notify,
0346 };
0347
0348 struct imx_media_dev *imx_media_dev_init(struct device *dev,
0349                      const struct media_device_ops *ops)
0350 {
0351     struct imx_media_dev *imxmd;
0352     int ret;
0353
0354     imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL);
0355     if (!imxmd)
0356         return ERR_PTR(-ENOMEM);
0357
0358     dev_set_drvdata(dev, imxmd);
0359
0360     strscpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model));
0361     imxmd->md.ops = ops ? ops : &imx_media_md_ops;
0362     imxmd->md.dev = dev;
0363
0364     mutex_init(&imxmd->mutex);
0365
0366     imxmd->v4l2_dev.mdev = &imxmd->md;
0367     imxmd->v4l2_dev.notify = imx_media_notify;
0368     strscpy(imxmd->v4l2_dev.name, "imx-media",
0369         sizeof(imxmd->v4l2_dev.name));
0370     snprintf(imxmd->md.bus_info, sizeof(imxmd->md.bus_info),
0371          "platform:%s", dev_name(imxmd->md.dev));
0372
0373     media_device_init(&imxmd->md);
0374
0375     ret = v4l2_device_register(dev, &imxmd->v4l2_dev);
0376     if (ret < 0) {
0377         v4l2_err(&imxmd->v4l2_dev,
0378              "Failed to register v4l2_device: %d\n", ret);
0379         goto cleanup;
0380     }
0381
0382     INIT_LIST_HEAD(&imxmd->vdev_list);
0383
0384     v4l2_async_nf_init(&imxmd->notifier);
0385
0386     return imxmd;
0387
0388 cleanup:
0389     media_device_cleanup(&imxmd->md);
0390
0391     return ERR_PTR(ret);
0392 }
0393 EXPORT_SYMBOL_GPL(imx_media_dev_init);
0394
0395 int imx_media_dev_notifier_register(struct imx_media_dev *imxmd,
0396                 const struct v4l2_async_notifier_operations *ops)
0397 {
0398     int ret;
0399
0400     /* no subdevs? just bail */
0401     if (list_empty(&imxmd->notifier.asd_list)) {
0402         v4l2_err(&imxmd->v4l2_dev, "no subdevs\n");
0403         return -ENODEV;
0404     }
0405
0406     /* prepare the async subdev notifier and register it */
0407     imxmd->notifier.ops = ops ? ops : &imx_media_notifier_ops;
0408     ret = v4l2_async_nf_register(&imxmd->v4l2_dev, &imxmd->notifier);
0409     if (ret) {
0410         v4l2_err(&imxmd->v4l2_dev,
0411              "v4l2_async_nf_register failed with %d\n", ret);
0412         return ret;
0413     }
0414
0415     return 0;
0416 }
0417 EXPORT_SYMBOL_GPL(imx_media_dev_notifier_register);