LXR linux/drivers/staging/media/imx/imx-media-dev-common.c

   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * V4L2 Media Controller Driver for Freescale common i.MX5/6/7 SOC
   4 *
   5 * Copyright (c) 2019 Linaro Ltd
   6 * Copyright (c) 2016 Mentor Graphics Inc.
   7 */
   8
   9#include <linux/of_graph.h>
  10#include <linux/of_platform.h>
  11#include <media/v4l2-ctrls.h>
  12#include <media/v4l2-event.h>
  13#include <media/v4l2-ioctl.h>
  14#include <media/v4l2-mc.h>
  15#include "imx-media.h"
  16
  17static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n)
  18{
  19        return container_of(n, struct imx_media_dev, notifier);
  20}
  21
  22/* async subdev bound notifier */
  23static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier,
  24                                  struct v4l2_subdev *sd,
  25                                  struct v4l2_async_subdev *asd)
  26{
  27        struct imx_media_dev *imxmd = notifier2dev(notifier);
  28
  29        dev_dbg(imxmd->md.dev, "subdev %s bound\n", sd->name);
  30
  31        return 0;
  32}
  33
  34/*
  35 * Create the missing media links from the CSI-2 receiver.
  36 * Called after all async subdevs have bound.
  37 */
  38static void imx_media_create_csi2_links(struct imx_media_dev *imxmd)
  39{
  40        struct v4l2_subdev *sd, *csi2 = NULL;
  41
  42        list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
  43                if (sd->grp_id == IMX_MEDIA_GRP_ID_CSI2) {
  44                        csi2 = sd;
  45                        break;
  46                }
  47        }
  48        if (!csi2)
  49                return;
  50
  51        list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
  52                /* skip if not a CSI or a CSI mux */
  53                if (!(sd->grp_id & IMX_MEDIA_GRP_ID_IPU_CSI) &&
  54                    !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI) &&
  55                    !(sd->grp_id & IMX_MEDIA_GRP_ID_CSI_MUX))
  56                        continue;
  57
  58                v4l2_create_fwnode_links(csi2, sd);
  59        }
  60}
  61
  62/*
  63 * adds given video device to given imx-media source pad vdev list.
  64 * Continues upstream from the pad entity's sink pads.
  65 */
  66static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd,
  67                                     struct imx_media_video_dev *vdev,
  68                                     struct media_pad *srcpad)
  69{
  70        struct media_entity *entity = srcpad->entity;
  71        struct imx_media_pad_vdev *pad_vdev;
  72        struct list_head *pad_vdev_list;
  73        struct media_link *link;
  74        struct v4l2_subdev *sd;
  75        int i, ret;
  76
  77        /* skip this entity if not a v4l2_subdev */
  78        if (!is_media_entity_v4l2_subdev(entity))
  79                return 0;
  80
  81        sd = media_entity_to_v4l2_subdev(entity);
  82
  83        pad_vdev_list = to_pad_vdev_list(sd, srcpad->index);
  84        if (!pad_vdev_list) {
  85                v4l2_warn(&imxmd->v4l2_dev, "%s:%u has no vdev list!\n",
  86                          entity->name, srcpad->index);
  87                /*
  88                 * shouldn't happen, but no reason to fail driver load,
  89                 * just skip this entity.
  90                 */
  91                return 0;
  92        }
  93
  94        /* just return if we've been here before */
  95        list_for_each_entry(pad_vdev, pad_vdev_list, list) {
  96                if (pad_vdev->vdev == vdev)
  97                        return 0;
  98        }
  99
 100        dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n",
 101                vdev->vfd->entity.name, entity->name, srcpad->index);
 102
 103        pad_vdev = devm_kzalloc(imxmd->md.dev, sizeof(*pad_vdev), GFP_KERNEL);
 104        if (!pad_vdev)
 105                return -ENOMEM;
 106
 107        /* attach this vdev to this pad */
 108        pad_vdev->vdev = vdev;
 109        list_add_tail(&pad_vdev->list, pad_vdev_list);
 110
 111        /* move upstream from this entity's sink pads */
 112        for (i = 0; i < entity->num_pads; i++) {
 113                struct media_pad *pad = &entity->pads[i];
 114
 115                if (!(pad->flags & MEDIA_PAD_FL_SINK))
 116                        continue;
 117
 118                list_for_each_entry(link, &entity->links, list) {
 119                        if (link->sink != pad)
 120                                continue;
 121                        ret = imx_media_add_vdev_to_pad(imxmd, vdev,
 122                                                        link->source);
 123                        if (ret)
 124                                return ret;
 125                }
 126        }
 127
 128        return 0;
 129}
 130
 131/*
 132 * For every subdevice, allocate an array of list_head's, one list_head
 133 * for each pad, to hold the list of video devices reachable from that
 134 * pad.
 135 */
 136static int imx_media_alloc_pad_vdev_lists(struct imx_media_dev *imxmd)
 137{
 138        struct list_head *vdev_lists;
 139        struct media_entity *entity;
 140        struct v4l2_subdev *sd;
 141        int i;
 142
 143        list_for_each_entry(sd, &imxmd->v4l2_dev.subdevs, list) {
 144                entity = &sd->entity;
 145                vdev_lists = devm_kcalloc(imxmd->md.dev,
 146                                          entity->num_pads, sizeof(*vdev_lists),
 147                                          GFP_KERNEL);
 148                if (!vdev_lists)
 149                        return -ENOMEM;
 150
 151                /* attach to the subdev's host private pointer */
 152                sd->host_priv = vdev_lists;
 153
 154                for (i = 0; i < entity->num_pads; i++)
 155                        INIT_LIST_HEAD(to_pad_vdev_list(sd, i));
 156        }
 157
 158        return 0;
 159}
 160
 161/* form the vdev lists in all imx-media source pads */
 162static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd)
 163{
 164        struct imx_media_video_dev *vdev;
 165        struct media_link *link;
 166        int ret;
 167
 168        ret = imx_media_alloc_pad_vdev_lists(imxmd);
 169        if (ret)
 170                return ret;
 171
 172        list_for_each_entry(vdev, &imxmd->vdev_list, list) {
 173                link = list_first_entry(&vdev->vfd->entity.links,
 174                                        struct media_link, list);
 175                ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source);
 176                if (ret)
 177                        return ret;
 178        }
 179
 180        return 0;
 181}
 182
 183/* async subdev complete notifier */
 184int imx_media_probe_complete(struct v4l2_async_notifier *notifier)
 185{
 186        struct imx_media_dev *imxmd = notifier2dev(notifier);
 187        int ret;
 188
 189        mutex_lock(&imxmd->mutex);
 190
 191        imx_media_create_csi2_links(imxmd);
 192
 193        ret = imx_media_create_pad_vdev_lists(imxmd);
 194        if (ret)
 195                goto unlock;
 196
 197        ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev);
 198unlock:
 199        mutex_unlock(&imxmd->mutex);
 200        if (ret)
 201                return ret;
 202
 203        return media_device_register(&imxmd->md);
 204}
 205EXPORT_SYMBOL_GPL(imx_media_probe_complete);
 206
 207/*
 208 * adds controls to a video device from an entity subdevice.
 209 * Continues upstream from the entity's sink pads.
 210 */
 211static int imx_media_inherit_controls(struct imx_media_dev *imxmd,
 212                                      struct video_device *vfd,
 213                                      struct media_entity *entity)
 214{
 215        int i, ret = 0;
 216
 217        if (is_media_entity_v4l2_subdev(entity)) {
 218                struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 219
 220                dev_dbg(imxmd->md.dev,
 221                        "adding controls to %s from %s\n",
 222                        vfd->entity.name, sd->entity.name);
 223
 224                ret = v4l2_ctrl_add_handler(vfd->ctrl_handler,
 225                                            sd->ctrl_handler,
 226                                            NULL, true);
 227                if (ret)
 228                        return ret;
 229        }
 230
 231        /* move upstream */
 232        for (i = 0; i < entity->num_pads; i++) {
 233                struct media_pad *pad, *spad = &entity->pads[i];
 234
 235                if (!(spad->flags & MEDIA_PAD_FL_SINK))
 236                        continue;
 237
 238                pad = media_entity_remote_pad(spad);
 239                if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 240                        continue;
 241
 242                ret = imx_media_inherit_controls(imxmd, vfd, pad->entity);
 243                if (ret)
 244                        break;
 245        }
 246
 247        return ret;
 248}
 249
 250static int imx_media_link_notify(struct media_link *link, u32 flags,
 251                                 unsigned int notification)
 252{
 253        struct imx_media_dev *imxmd = container_of(link->graph_obj.mdev,
 254                                                   struct imx_media_dev, md);
 255        struct media_entity *source = link->source->entity;
 256        struct imx_media_pad_vdev *pad_vdev;
 257        struct list_head *pad_vdev_list;
 258        struct video_device *vfd;
 259        struct v4l2_subdev *sd;
 260        int pad_idx, ret;
 261
 262        ret = v4l2_pipeline_link_notify(link, flags, notification);
 263        if (ret)
 264                return ret;
 265
 266        /* don't bother if source is not a subdev */
 267        if (!is_media_entity_v4l2_subdev(source))
 268                return 0;
 269
 270        sd = media_entity_to_v4l2_subdev(source);
 271        pad_idx = link->source->index;
 272
 273        pad_vdev_list = to_pad_vdev_list(sd, pad_idx);
 274        if (!pad_vdev_list) {
 275                /* nothing to do if source sd has no pad vdev list */
 276                return 0;
 277        }
 278
 279        /*
 280         * Before disabling a link, reset controls for all video
 281         * devices reachable from this link.
 282         *
 283         * After enabling a link, refresh controls for all video
 284         * devices reachable from this link.
 285         */
 286        if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
 287            !(flags & MEDIA_LNK_FL_ENABLED)) {
 288                list_for_each_entry(pad_vdev, pad_vdev_list, list) {
 289                        vfd = pad_vdev->vdev->vfd;
 290                        if (!vfd->ctrl_handler)
 291                                continue;
 292                        dev_dbg(imxmd->md.dev,
 293                                "reset controls for %s\n",
 294                                vfd->entity.name);
 295                        v4l2_ctrl_handler_free(vfd->ctrl_handler);
 296                        v4l2_ctrl_handler_init(vfd->ctrl_handler, 0);
 297                }
 298        } else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
 299                   (link->flags & MEDIA_LNK_FL_ENABLED)) {
 300                list_for_each_entry(pad_vdev, pad_vdev_list, list) {
 301                        vfd = pad_vdev->vdev->vfd;
 302                        if (!vfd->ctrl_handler)
 303                                continue;
 304                        dev_dbg(imxmd->md.dev,
 305                                "refresh controls for %s\n",
 306                                vfd->entity.name);
 307                        ret = imx_media_inherit_controls(imxmd, vfd,
 308                                                         &vfd->entity);
 309                        if (ret)
 310                                break;
 311                }
 312        }
 313
 314        return ret;
 315}
 316
 317static void imx_media_notify(struct v4l2_subdev *sd, unsigned int notification,
 318                             void *arg)
 319{
 320        struct media_entity *entity = &sd->entity;
 321        int i;
 322
 323        if (notification != V4L2_DEVICE_NOTIFY_EVENT)
 324                return;
 325
 326        for (i = 0; i < entity->num_pads; i++) {
 327                struct media_pad *pad = &entity->pads[i];
 328                struct imx_media_pad_vdev *pad_vdev;
 329                struct list_head *pad_vdev_list;
 330
 331                pad_vdev_list = to_pad_vdev_list(sd, pad->index);
 332                if (!pad_vdev_list)
 333                        continue;
 334                list_for_each_entry(pad_vdev, pad_vdev_list, list)
 335                        v4l2_event_queue(pad_vdev->vdev->vfd, arg);
 336        }
 337}
 338
 339static const struct v4l2_async_notifier_operations imx_media_notifier_ops = {
 340        .bound = imx_media_subdev_bound,
 341        .complete = imx_media_probe_complete,
 342};
 343
 344static const struct media_device_ops imx_media_md_ops = {
 345        .link_notify = imx_media_link_notify,
 346};
 347
 348struct imx_media_dev *imx_media_dev_init(struct device *dev,
 349                                         const struct media_device_ops *ops)
 350{
 351        struct imx_media_dev *imxmd;
 352        int ret;
 353
 354        imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL);
 355        if (!imxmd)
 356                return ERR_PTR(-ENOMEM);
 357
 358        dev_set_drvdata(dev, imxmd);
 359
 360        strscpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model));
 361        imxmd->md.ops = ops ? ops : &imx_media_md_ops;
 362        imxmd->md.dev = dev;
 363
 364        mutex_init(&imxmd->mutex);
 365
 366        imxmd->v4l2_dev.mdev = &imxmd->md;
 367        imxmd->v4l2_dev.notify = imx_media_notify;
 368        strscpy(imxmd->v4l2_dev.name, "imx-media",
 369                sizeof(imxmd->v4l2_dev.name));
 370
 371        media_device_init(&imxmd->md);
 372
 373        ret = v4l2_device_register(dev, &imxmd->v4l2_dev);
 374        if (ret < 0) {
 375                v4l2_err(&imxmd->v4l2_dev,
 376                         "Failed to register v4l2_device: %d\n", ret);
 377                goto cleanup;
 378        }
 379
 380        INIT_LIST_HEAD(&imxmd->vdev_list);
 381
 382        v4l2_async_notifier_init(&imxmd->notifier);
 383
 384        return imxmd;
 385
 386cleanup:
 387        media_device_cleanup(&imxmd->md);
 388
 389        return ERR_PTR(ret);
 390}
 391EXPORT_SYMBOL_GPL(imx_media_dev_init);
 392
 393int imx_media_dev_notifier_register(struct imx_media_dev *imxmd,
 394                            const struct v4l2_async_notifier_operations *ops)
 395{
 396        int ret;
 397
 398        /* no subdevs? just bail */
 399        if (list_empty(&imxmd->notifier.asd_list)) {
 400                v4l2_err(&imxmd->v4l2_dev, "no subdevs\n");
 401                return -ENODEV;
 402        }
 403
 404        /* prepare the async subdev notifier and register it */
 405        imxmd->notifier.ops = ops ? ops : &imx_media_notifier_ops;
 406        ret = v4l2_async_notifier_register(&imxmd->v4l2_dev,
 407                                           &imxmd->notifier);
 408        if (ret) {
 409                v4l2_err(&imxmd->v4l2_dev,
 410                         "v4l2_async_notifier_register failed with %d\n", ret);
 411                return ret;
 412        }
 413
 414        return 0;
 415}
 416EXPORT_SYMBOL_GPL(imx_media_dev_notifier_register);
 417