drivers/staging/media/imx/imx-media-dev.c

   1 /*
   2  * V4L2 Media Controller Driver for Freescale i.MX5/6 SOC
   3  *
   4  * Copyright (c) 2016 Mentor Graphics Inc.
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  */
  11 #include <linux/delay.h>
  12 #include <linux/fs.h>
  13 #include <linux/module.h>
  14 #include <linux/of_graph.h>
  15 #include <linux/of_platform.h>
  16 #include <linux/pinctrl/consumer.h>
  17 #include <linux/platform_device.h>
  18 #include <linux/sched.h>
  19 #include <linux/slab.h>
  20 #include <linux/spinlock.h>
  21 #include <linux/timer.h>
  22 #include <media/v4l2-ctrls.h>
  23 #include <media/v4l2-event.h>
  24 #include <media/v4l2-ioctl.h>
  25 #include <media/v4l2-mc.h>
  26 #include <video/imx-ipu-v3.h>
  27 #include <media/imx.h>
  28 #include "imx-media.h"
  29
  30 static inline struct imx_media_dev *notifier2dev(struct v4l2_async_notifier *n)
  31 {
  32         return container_of(n, struct imx_media_dev, subdev_notifier);
  33 }
  34
  35 /*
  36  * Find a subdev by device node or device name. This is called during
  37  * driver load to form the async subdev list and bind them.
  38  */
  39 struct imx_media_subdev *
  40 imx_media_find_async_subdev(struct imx_media_dev *imxmd,
  41                             struct device_node *np,
  42                             const char *devname)
  43 {
  44         struct fwnode_handle *fwnode = np ? of_fwnode_handle(np) : NULL;
  45         struct imx_media_subdev *imxsd;
  46         int i;
  47
  48         for (i = 0; i < imxmd->subdev_notifier.num_subdevs; i++) {
  49                 imxsd = &imxmd->subdev[i];
  50                 switch (imxsd->asd.match_type) {
  51                 case V4L2_ASYNC_MATCH_FWNODE:
  52                         if (fwnode && imxsd->asd.match.fwnode.fwnode == fwnode)
  53                                 return imxsd;
  54                         break;
  55                 case V4L2_ASYNC_MATCH_DEVNAME:
  56                         if (devname &&
  57                             !strcmp(imxsd->asd.match.device_name.name, devname))
  58                                 return imxsd;
  59                         break;
  60                 default:
  61                         break;
  62                 }
  63         }
  64
  65         return NULL;
  66 }
  67
  68
  69 /*
  70  * Adds a subdev to the async subdev list. If np is non-NULL, adds
  71  * the async as a V4L2_ASYNC_MATCH_FWNODE match type, otherwise as
  72  * a V4L2_ASYNC_MATCH_DEVNAME match type using the dev_name of the
  73  * given platform_device. This is called during driver load when
  74  * forming the async subdev list.
  75  */
  76 struct imx_media_subdev *
  77 imx_media_add_async_subdev(struct imx_media_dev *imxmd,
  78                            struct device_node *np,
  79                            struct platform_device *pdev)
  80 {
  81         struct imx_media_subdev *imxsd;
  82         struct v4l2_async_subdev *asd;
  83         const char *devname = NULL;
  84         int sd_idx;
  85
  86         mutex_lock(&imxmd->mutex);
  87
  88         if (pdev)
  89                 devname = dev_name(&pdev->dev);
  90
  91         /* return -EEXIST if this subdev already added */
  92         if (imx_media_find_async_subdev(imxmd, np, devname)) {
  93                 dev_dbg(imxmd->md.dev, "%s: already added %s\n",
  94                         __func__, np ? np->name : devname);
  95                 imxsd = ERR_PTR(-EEXIST);
  96                 goto out;
  97         }
  98
  99         sd_idx = imxmd->subdev_notifier.num_subdevs;
 100         if (sd_idx >= IMX_MEDIA_MAX_SUBDEVS) {
 101                 dev_err(imxmd->md.dev, "%s: too many subdevs! can't add %s\n",
 102                         __func__, np ? np->name : devname);
 103                 imxsd = ERR_PTR(-ENOSPC);
 104                 goto out;
 105         }
 106
 107         imxsd = &imxmd->subdev[sd_idx];
 108
 109         asd = &imxsd->asd;
 110         if (np) {
 111                 asd->match_type = V4L2_ASYNC_MATCH_FWNODE;
 112                 asd->match.fwnode.fwnode = of_fwnode_handle(np);
 113         } else {
 114                 asd->match_type = V4L2_ASYNC_MATCH_DEVNAME;
 115                 asd->match.device_name.name = devname;
 116                 imxsd->pdev = pdev;
 117         }
 118
 119         imxmd->async_ptrs[sd_idx] = asd;
 120         imxmd->subdev_notifier.num_subdevs++;
 121
 122         dev_dbg(imxmd->md.dev, "%s: added %s, match type %s\n",
 123                 __func__, np ? np->name : devname, np ? "FWNODE" : "DEVNAME");
 124
 125 out:
 126         mutex_unlock(&imxmd->mutex);
 127         return imxsd;
 128 }
 129
 130 /*
 131  * get IPU from this CSI and add it to the list of IPUs
 132  * the media driver will control.
 133  */
 134 static int imx_media_get_ipu(struct imx_media_dev *imxmd,
 135                              struct v4l2_subdev *csi_sd)
 136 {
 137         struct ipu_soc *ipu;
 138         int ipu_id;
 139
 140         ipu = dev_get_drvdata(csi_sd->dev->parent);
 141         if (!ipu) {
 142                 v4l2_err(&imxmd->v4l2_dev,
 143                          "CSI %s has no parent IPU!\n", csi_sd->name);
 144                 return -ENODEV;
 145         }
 146
 147         ipu_id = ipu_get_num(ipu);
 148         if (ipu_id > 1) {
 149                 v4l2_err(&imxmd->v4l2_dev, "invalid IPU id %d!\n", ipu_id);
 150                 return -ENODEV;
 151         }
 152
 153         if (!imxmd->ipu[ipu_id])
 154                 imxmd->ipu[ipu_id] = ipu;
 155
 156         return 0;
 157 }
 158
 159 /* async subdev bound notifier */
 160 static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier,
 161                                   struct v4l2_subdev *sd,
 162                                   struct v4l2_async_subdev *asd)
 163 {
 164         struct imx_media_dev *imxmd = notifier2dev(notifier);
 165         struct device_node *np = to_of_node(sd->fwnode);
 166         struct imx_media_subdev *imxsd;
 167         int ret = 0;
 168
 169         mutex_lock(&imxmd->mutex);
 170
 171         imxsd = imx_media_find_async_subdev(imxmd, np, dev_name(sd->dev));
 172         if (!imxsd) {
 173                 ret = -EINVAL;
 174                 goto out;
 175         }
 176
 177         if (sd->grp_id & IMX_MEDIA_GRP_ID_CSI) {
 178                 ret = imx_media_get_ipu(imxmd, sd);
 179                 if (ret)
 180                         goto out_unlock;
 181         }
 182
 183         /* attach the subdev */
 184         imxsd->sd = sd;
 185 out:
 186         if (ret)
 187                 v4l2_warn(&imxmd->v4l2_dev,
 188                           "Received unknown subdev %s\n", sd->name);
 189         else
 190                 v4l2_info(&imxmd->v4l2_dev,
 191                           "Registered subdev %s\n", sd->name);
 192
 193 out_unlock:
 194         mutex_unlock(&imxmd->mutex);
 195         return ret;
 196 }
 197
 198 /*
 199  * create the media links from all pads and their links.
 200  * Called after all subdevs have registered.
 201  */
 202 static int imx_media_create_links(struct imx_media_dev *imxmd)
 203 {
 204         struct imx_media_subdev *imxsd;
 205         struct v4l2_subdev *sd;
 206         int i, ret;
 207
 208         for (i = 0; i < imxmd->num_subdevs; i++) {
 209                 imxsd = &imxmd->subdev[i];
 210                 sd = imxsd->sd;
 211
 212                 if (((sd->grp_id & IMX_MEDIA_GRP_ID_CSI) || imxsd->pdev)) {
 213                         /* this is an internal subdev or a CSI */
 214                         ret = imx_media_create_internal_links(imxmd, imxsd);
 215                         if (ret)
 216                                 return ret;
 217                         /*
 218                          * the CSIs straddle between the external and the IPU
 219                          * internal entities, so create the external links
 220                          * to the CSI sink pads.
 221                          */
 222                         if (sd->grp_id & IMX_MEDIA_GRP_ID_CSI)
 223                                 imx_media_create_csi_of_links(imxmd, imxsd);
 224                 } else {
 225                         /* this is an external fwnode subdev */
 226                         imx_media_create_of_links(imxmd, imxsd);
 227                 }
 228         }
 229
 230         return 0;
 231 }
 232
 233 /*
 234  * adds given video device to given imx-media source pad vdev list.
 235  * Continues upstream from the pad entity's sink pads.
 236  */
 237 static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd,
 238                                      struct imx_media_video_dev *vdev,
 239                                      struct media_pad *srcpad)
 240 {
 241         struct media_entity *entity = srcpad->entity;
 242         struct imx_media_subdev *imxsd;
 243         struct imx_media_pad *imxpad;
 244         struct media_link *link;
 245         struct v4l2_subdev *sd;
 246         int i, vdev_idx, ret;
 247
 248         /* skip this entity if not a v4l2_subdev */
 249         if (!is_media_entity_v4l2_subdev(entity))
 250                 return 0;
 251
 252         sd = media_entity_to_v4l2_subdev(entity);
 253         imxsd = imx_media_find_subdev_by_sd(imxmd, sd);
 254         if (IS_ERR(imxsd)) {
 255                 v4l2_err(&imxmd->v4l2_dev, "failed to find subdev for entity %s, sd %p err %ld\n",
 256                          entity->name, sd, PTR_ERR(imxsd));
 257                 return 0;
 258         }
 259
 260         imxpad = &imxsd->pad[srcpad->index];
 261         vdev_idx = imxpad->num_vdevs;
 262
 263         /* just return if we've been here before */
 264         for (i = 0; i < vdev_idx; i++)
 265                 if (vdev == imxpad->vdev[i])
 266                         return 0;
 267
 268         if (vdev_idx >= IMX_MEDIA_MAX_VDEVS) {
 269                 dev_err(imxmd->md.dev, "can't add %s to pad %s:%u\n",
 270                         vdev->vfd->entity.name, entity->name, srcpad->index);
 271                 return -ENOSPC;
 272         }
 273
 274         dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n",
 275                 vdev->vfd->entity.name, entity->name, srcpad->index);
 276         imxpad->vdev[vdev_idx] = vdev;
 277         imxpad->num_vdevs++;
 278
 279         /* move upstream from this entity's sink pads */
 280         for (i = 0; i < entity->num_pads; i++) {
 281                 struct media_pad *pad = &entity->pads[i];
 282
 283                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 284                         continue;
 285
 286                 list_for_each_entry(link, &entity->links, list) {
 287                         if (link->sink != pad)
 288                                 continue;
 289                         ret = imx_media_add_vdev_to_pad(imxmd, vdev,
 290                                                         link->source);
 291                         if (ret)
 292                                 return ret;
 293                 }
 294         }
 295
 296         return 0;
 297 }
 298
 299 /* form the vdev lists in all imx-media source pads */
 300 static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd)
 301 {
 302         struct imx_media_video_dev *vdev;
 303         struct media_link *link;
 304         int i, ret;
 305
 306         for (i = 0; i < imxmd->num_vdevs; i++) {
 307                 vdev = imxmd->vdev[i];
 308                 link = list_first_entry(&vdev->vfd->entity.links,
 309                                         struct media_link, list);
 310                 ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source);
 311                 if (ret)
 312                         return ret;
 313         }
 314
 315         return 0;
 316 }
 317
 318 /* async subdev complete notifier */
 319 static int imx_media_probe_complete(struct v4l2_async_notifier *notifier)
 320 {
 321         struct imx_media_dev *imxmd = notifier2dev(notifier);
 322         int i, ret;
 323
 324         mutex_lock(&imxmd->mutex);
 325
 326         /* make sure all subdevs were bound */
 327         for (i = 0; i < imxmd->num_subdevs; i++) {
 328                 if (!imxmd->subdev[i].sd) {
 329                         v4l2_err(&imxmd->v4l2_dev, "unbound subdev!\n");
 330                         ret = -ENODEV;
 331                         goto unlock;
 332                 }
 333         }
 334
 335         ret = imx_media_create_links(imxmd);
 336         if (ret)
 337                 goto unlock;
 338
 339         ret = imx_media_create_pad_vdev_lists(imxmd);
 340         if (ret)
 341                 goto unlock;
 342
 343         ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev);
 344 unlock:
 345         mutex_unlock(&imxmd->mutex);
 346         if (ret)
 347                 return ret;
 348
 349         return media_device_register(&imxmd->md);
 350 }
 351
 352 static const struct v4l2_async_notifier_operations imx_media_subdev_ops = {
 353         .bound = imx_media_subdev_bound,
 354         .complete = imx_media_probe_complete,
 355 };
 356
 357 /*
 358  * adds controls to a video device from an entity subdevice.
 359  * Continues upstream from the entity's sink pads.
 360  */
 361 static int imx_media_inherit_controls(struct imx_media_dev *imxmd,
 362                                       struct video_device *vfd,
 363                                       struct media_entity *entity)
 364 {
 365         int i, ret = 0;
 366
 367         if (is_media_entity_v4l2_subdev(entity)) {
 368                 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 369
 370                 dev_dbg(imxmd->md.dev,
 371                         "adding controls to %s from %s\n",
 372                         vfd->entity.name, sd->entity.name);
 373
 374                 ret = v4l2_ctrl_add_handler(vfd->ctrl_handler,
 375                                             sd->ctrl_handler,
 376                                             NULL);
 377                 if (ret)
 378                         return ret;
 379         }
 380
 381         /* move upstream */
 382         for (i = 0; i < entity->num_pads; i++) {
 383                 struct media_pad *pad, *spad = &entity->pads[i];
 384
 385                 if (!(spad->flags & MEDIA_PAD_FL_SINK))
 386                         continue;
 387
 388                 pad = media_entity_remote_pad(spad);
 389                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 390                         continue;
 391
 392                 ret = imx_media_inherit_controls(imxmd, vfd, pad->entity);
 393                 if (ret)
 394                         break;
 395         }
 396
 397         return ret;
 398 }
 399
 400 static int imx_media_link_notify(struct media_link *link, u32 flags,
 401                                  unsigned int notification)
 402 {
 403         struct media_entity *source = link->source->entity;
 404         struct imx_media_subdev *imxsd;
 405         struct imx_media_pad *imxpad;
 406         struct imx_media_dev *imxmd;
 407         struct video_device *vfd;
 408         struct v4l2_subdev *sd;
 409         int i, pad_idx, ret;
 410
 411         ret = v4l2_pipeline_link_notify(link, flags, notification);
 412         if (ret)
 413                 return ret;
 414
 415         /* don't bother if source is not a subdev */
 416         if (!is_media_entity_v4l2_subdev(source))
 417                 return 0;
 418
 419         sd = media_entity_to_v4l2_subdev(source);
 420         pad_idx = link->source->index;
 421
 422         imxmd = dev_get_drvdata(sd->v4l2_dev->dev);
 423
 424         imxsd = imx_media_find_subdev_by_sd(imxmd, sd);
 425         if (IS_ERR(imxsd))
 426                 return PTR_ERR(imxsd);
 427         imxpad = &imxsd->pad[pad_idx];
 428
 429         /*
 430          * Before disabling a link, reset controls for all video
 431          * devices reachable from this link.
 432          *
 433          * After enabling a link, refresh controls for all video
 434          * devices reachable from this link.
 435          */
 436         if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
 437             !(flags & MEDIA_LNK_FL_ENABLED)) {
 438                 for (i = 0; i < imxpad->num_vdevs; i++) {
 439                         vfd = imxpad->vdev[i]->vfd;
 440                         dev_dbg(imxmd->md.dev,
 441                                 "reset controls for %s\n",
 442                                 vfd->entity.name);
 443                         v4l2_ctrl_handler_free(vfd->ctrl_handler);
 444                         v4l2_ctrl_handler_init(vfd->ctrl_handler, 0);
 445                 }
 446         } else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
 447                    (link->flags & MEDIA_LNK_FL_ENABLED)) {
 448                 for (i = 0; i < imxpad->num_vdevs; i++) {
 449                         vfd = imxpad->vdev[i]->vfd;
 450                         dev_dbg(imxmd->md.dev,
 451                                 "refresh controls for %s\n",
 452                                 vfd->entity.name);
 453                         ret = imx_media_inherit_controls(imxmd, vfd,
 454                                                          &vfd->entity);
 455                         if (ret)
 456                                 break;
 457                 }
 458         }
 459
 460         return ret;
 461 }
 462
 463 static const struct media_device_ops imx_media_md_ops = {
 464         .link_notify = imx_media_link_notify,
 465 };
 466
 467 static int imx_media_probe(struct platform_device *pdev)
 468 {
 469         struct device *dev = &pdev->dev;
 470         struct device_node *node = dev->of_node;
 471         struct imx_media_dev *imxmd;
 472         int ret;
 473
 474         imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL);
 475         if (!imxmd)
 476                 return -ENOMEM;
 477
 478         dev_set_drvdata(dev, imxmd);
 479
 480         strlcpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model));
 481         imxmd->md.ops = &imx_media_md_ops;
 482         imxmd->md.dev = dev;
 483
 484         mutex_init(&imxmd->mutex);
 485
 486         imxmd->v4l2_dev.mdev = &imxmd->md;
 487         strlcpy(imxmd->v4l2_dev.name, "imx-media",
 488                 sizeof(imxmd->v4l2_dev.name));
 489
 490         media_device_init(&imxmd->md);
 491
 492         ret = v4l2_device_register(dev, &imxmd->v4l2_dev);
 493         if (ret < 0) {
 494                 v4l2_err(&imxmd->v4l2_dev,
 495                          "Failed to register v4l2_device: %d\n", ret);
 496                 goto cleanup;
 497         }
 498
 499         dev_set_drvdata(imxmd->v4l2_dev.dev, imxmd);
 500
 501         ret = imx_media_add_of_subdevs(imxmd, node);
 502         if (ret) {
 503                 v4l2_err(&imxmd->v4l2_dev,
 504                          "add_of_subdevs failed with %d\n", ret);
 505                 goto unreg_dev;
 506         }
 507
 508         ret = imx_media_add_internal_subdevs(imxmd);
 509         if (ret) {
 510                 v4l2_err(&imxmd->v4l2_dev,
 511                          "add_internal_subdevs failed with %d\n", ret);
 512                 goto unreg_dev;
 513         }
 514
 515         /* no subdevs? just bail */
 516         imxmd->num_subdevs = imxmd->subdev_notifier.num_subdevs;
 517         if (imxmd->num_subdevs == 0) {
 518                 ret = -ENODEV;
 519                 goto unreg_dev;
 520         }
 521
 522         /* prepare the async subdev notifier and register it */
 523         imxmd->subdev_notifier.subdevs = imxmd->async_ptrs;
 524         imxmd->subdev_notifier.ops = &imx_media_subdev_ops;
 525         ret = v4l2_async_notifier_register(&imxmd->v4l2_dev,
 526                                            &imxmd->subdev_notifier);
 527         if (ret) {
 528                 v4l2_err(&imxmd->v4l2_dev,
 529                          "v4l2_async_notifier_register failed with %d\n", ret);
 530                 goto del_int;
 531         }
 532
 533         return 0;
 534
 535 del_int:
 536         imx_media_remove_internal_subdevs(imxmd);
 537 unreg_dev:
 538         v4l2_device_unregister(&imxmd->v4l2_dev);
 539 cleanup:
 540         media_device_cleanup(&imxmd->md);
 541         return ret;
 542 }
 543
 544 static int imx_media_remove(struct platform_device *pdev)
 545 {
 546         struct imx_media_dev *imxmd =
 547                 (struct imx_media_dev *)platform_get_drvdata(pdev);
 548
 549         v4l2_info(&imxmd->v4l2_dev, "Removing imx-media\n");
 550
 551         v4l2_async_notifier_unregister(&imxmd->subdev_notifier);
 552         imx_media_remove_internal_subdevs(imxmd);
 553         v4l2_device_unregister(&imxmd->v4l2_dev);
 554         media_device_unregister(&imxmd->md);
 555         media_device_cleanup(&imxmd->md);
 556
 557         return 0;
 558 }
 559
 560 static const struct of_device_id imx_media_dt_ids[] = {
 561         { .compatible = "fsl,imx-capture-subsystem" },
 562         { /* sentinel */ }
 563 };
 564 MODULE_DEVICE_TABLE(of, imx_media_dt_ids);
 565
 566 static struct platform_driver imx_media_pdrv = {
 567         .probe          = imx_media_probe,
 568         .remove         = imx_media_remove,
 569         .driver         = {
 570                 .name   = "imx-media",
 571                 .of_match_table = imx_media_dt_ids,
 572         },
 573 };
 574
 575 module_platform_driver(imx_media_pdrv);
 576
 577 MODULE_DESCRIPTION("i.MX5/6 v4l2 media controller driver");
 578 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
 579 MODULE_LICENSE("GPL");