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