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
  17 static 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 */
  23 static 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  */
  38 static 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  */
  66 static 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  */
 136 static 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 */
 162 static 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 */
 184 int 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);
 198 unlock:
 199         mutex_unlock(&imxmd->mutex);
 200         if (ret)
 201                 return ret;
 202
 203         return media_device_register(&imxmd->md);
 204 }
 205 EXPORT_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  */
 211 static 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
 250 static 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
 317 static 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
 339 static const struct v4l2_async_notifier_operations imx_media_notifier_ops = {
 340         .bound = imx_media_subdev_bound,
 341         .complete = imx_media_probe_complete,
 342 };
 343
 344 static const struct media_device_ops imx_media_md_ops = {
 345         .link_notify = imx_media_link_notify,
 346 };
 347
 348 struct 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         snprintf(imxmd->md.bus_info, sizeof(imxmd->md.bus_info),
 371                  "platform:%s", dev_name(imxmd->md.dev));
 372
 373         media_device_init(&imxmd->md);
 374
 375         ret = v4l2_device_register(dev, &imxmd->v4l2_dev);
 376         if (ret < 0) {
 377                 v4l2_err(&imxmd->v4l2_dev,
 378                          "Failed to register v4l2_device: %d\n", ret);
 379                 goto cleanup;
 380         }
 381
 382         INIT_LIST_HEAD(&imxmd->vdev_list);
 383
 384         v4l2_async_nf_init(&imxmd->notifier);
 385
 386         return imxmd;
 387
 388 cleanup:
 389         media_device_cleanup(&imxmd->md);
 390
 391         return ERR_PTR(ret);
 392 }
 393 EXPORT_SYMBOL_GPL(imx_media_dev_init);
 394
 395 int imx_media_dev_notifier_register(struct imx_media_dev *imxmd,
 396                             const struct v4l2_async_notifier_operations *ops)
 397 {
 398         int ret;
 399
 400         /* no subdevs? just bail */
 401         if (list_empty(&imxmd->notifier.asd_list)) {
 402                 v4l2_err(&imxmd->v4l2_dev, "no subdevs\n");
 403                 return -ENODEV;
 404         }
 405
 406         /* prepare the async subdev notifier and register it */
 407         imxmd->notifier.ops = ops ? ops : &imx_media_notifier_ops;
 408         ret = v4l2_async_nf_register(&imxmd->v4l2_dev, &imxmd->notifier);
 409         if (ret) {
 410                 v4l2_err(&imxmd->v4l2_dev,
 411                          "v4l2_async_nf_register failed with %d\n", ret);
 412                 return ret;
 413         }
 414
 415         return 0;
 416 }
 417 EXPORT_SYMBOL_GPL(imx_media_dev_notifier_register);