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                 strncpy(imxsd->devname, devname, sizeof(imxsd->devname));
 116                 asd->match.device_name.name = imxsd->devname;
 117                 imxsd->pdev = pdev;
 118         }
 119
 120         imxmd->async_ptrs[sd_idx] = asd;
 121         imxmd->subdev_notifier.num_subdevs++;
 122
 123         dev_dbg(imxmd->md.dev, "%s: added %s, match type %s\n",
 124                 __func__, np ? np->name : devname, np ? "FWNODE" : "DEVNAME");
 125
 126 out:
 127         mutex_unlock(&imxmd->mutex);
 128         return imxsd;
 129 }
 130
 131 /*
 132  * get IPU from this CSI and add it to the list of IPUs
 133  * the media driver will control.
 134  */
 135 static int imx_media_get_ipu(struct imx_media_dev *imxmd,
 136                              struct v4l2_subdev *csi_sd)
 137 {
 138         struct ipu_soc *ipu;
 139         int ipu_id;
 140
 141         ipu = dev_get_drvdata(csi_sd->dev->parent);
 142         if (!ipu) {
 143                 v4l2_err(&imxmd->v4l2_dev,
 144                          "CSI %s has no parent IPU!\n", csi_sd->name);
 145                 return -ENODEV;
 146         }
 147
 148         ipu_id = ipu_get_num(ipu);
 149         if (ipu_id > 1) {
 150                 v4l2_err(&imxmd->v4l2_dev, "invalid IPU id %d!\n", ipu_id);
 151                 return -ENODEV;
 152         }
 153
 154         if (!imxmd->ipu[ipu_id])
 155                 imxmd->ipu[ipu_id] = ipu;
 156
 157         return 0;
 158 }
 159
 160 /* async subdev bound notifier */
 161 static int imx_media_subdev_bound(struct v4l2_async_notifier *notifier,
 162                                   struct v4l2_subdev *sd,
 163                                   struct v4l2_async_subdev *asd)
 164 {
 165         struct imx_media_dev *imxmd = notifier2dev(notifier);
 166         struct device_node *np = to_of_node(sd->fwnode);
 167         struct imx_media_subdev *imxsd;
 168         int ret = 0;
 169
 170         mutex_lock(&imxmd->mutex);
 171
 172         imxsd = imx_media_find_async_subdev(imxmd, np, dev_name(sd->dev));
 173         if (!imxsd) {
 174                 ret = -EINVAL;
 175                 goto out;
 176         }
 177
 178         if (sd->grp_id & IMX_MEDIA_GRP_ID_CSI) {
 179                 ret = imx_media_get_ipu(imxmd, sd);
 180                 if (ret)
 181                         goto out_unlock;
 182         }
 183
 184         /* attach the subdev */
 185         imxsd->sd = sd;
 186 out:
 187         if (ret)
 188                 v4l2_warn(&imxmd->v4l2_dev,
 189                           "Received unknown subdev %s\n", sd->name);
 190         else
 191                 v4l2_info(&imxmd->v4l2_dev,
 192                           "Registered subdev %s\n", sd->name);
 193
 194 out_unlock:
 195         mutex_unlock(&imxmd->mutex);
 196         return ret;
 197 }
 198
 199 /*
 200  * create the media links from all pads and their links.
 201  * Called after all subdevs have registered.
 202  */
 203 static int imx_media_create_links(struct imx_media_dev *imxmd)
 204 {
 205         struct imx_media_subdev *imxsd;
 206         struct v4l2_subdev *sd;
 207         int i, ret;
 208
 209         for (i = 0; i < imxmd->num_subdevs; i++) {
 210                 imxsd = &imxmd->subdev[i];
 211                 sd = imxsd->sd;
 212
 213                 if (((sd->grp_id & IMX_MEDIA_GRP_ID_CSI) || imxsd->pdev)) {
 214                         /* this is an internal subdev or a CSI */
 215                         ret = imx_media_create_internal_links(imxmd, imxsd);
 216                         if (ret)
 217                                 return ret;
 218                         /*
 219                          * the CSIs straddle between the external and the IPU
 220                          * internal entities, so create the external links
 221                          * to the CSI sink pads.
 222                          */
 223                         if (sd->grp_id & IMX_MEDIA_GRP_ID_CSI)
 224                                 imx_media_create_csi_of_links(imxmd, imxsd);
 225                 } else {
 226                         /* this is an external fwnode subdev */
 227                         imx_media_create_of_links(imxmd, imxsd);
 228                 }
 229         }
 230
 231         return 0;
 232 }
 233
 234 /*
 235  * adds given video device to given imx-media source pad vdev list.
 236  * Continues upstream from the pad entity's sink pads.
 237  */
 238 static int imx_media_add_vdev_to_pad(struct imx_media_dev *imxmd,
 239                                      struct imx_media_video_dev *vdev,
 240                                      struct media_pad *srcpad)
 241 {
 242         struct media_entity *entity = srcpad->entity;
 243         struct imx_media_subdev *imxsd;
 244         struct imx_media_pad *imxpad;
 245         struct media_link *link;
 246         struct v4l2_subdev *sd;
 247         int i, vdev_idx, ret;
 248
 249         /* skip this entity if not a v4l2_subdev */
 250         if (!is_media_entity_v4l2_subdev(entity))
 251                 return 0;
 252
 253         sd = media_entity_to_v4l2_subdev(entity);
 254         imxsd = imx_media_find_subdev_by_sd(imxmd, sd);
 255         if (IS_ERR(imxsd)) {
 256                 v4l2_err(&imxmd->v4l2_dev, "failed to find subdev for entity %s, sd %p err %ld\n",
 257                          entity->name, sd, PTR_ERR(imxsd));
 258                 return 0;
 259         }
 260
 261         imxpad = &imxsd->pad[srcpad->index];
 262         vdev_idx = imxpad->num_vdevs;
 263
 264         /* just return if we've been here before */
 265         for (i = 0; i < vdev_idx; i++)
 266                 if (vdev == imxpad->vdev[i])
 267                         return 0;
 268
 269         if (vdev_idx >= IMX_MEDIA_MAX_VDEVS) {
 270                 dev_err(imxmd->md.dev, "can't add %s to pad %s:%u\n",
 271                         vdev->vfd->entity.name, entity->name, srcpad->index);
 272                 return -ENOSPC;
 273         }
 274
 275         dev_dbg(imxmd->md.dev, "adding %s to pad %s:%u\n",
 276                 vdev->vfd->entity.name, entity->name, srcpad->index);
 277         imxpad->vdev[vdev_idx] = vdev;
 278         imxpad->num_vdevs++;
 279
 280         /* move upstream from this entity's sink pads */
 281         for (i = 0; i < entity->num_pads; i++) {
 282                 struct media_pad *pad = &entity->pads[i];
 283
 284                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 285                         continue;
 286
 287                 list_for_each_entry(link, &entity->links, list) {
 288                         if (link->sink != pad)
 289                                 continue;
 290                         ret = imx_media_add_vdev_to_pad(imxmd, vdev,
 291                                                         link->source);
 292                         if (ret)
 293                                 return ret;
 294                 }
 295         }
 296
 297         return 0;
 298 }
 299
 300 /* form the vdev lists in all imx-media source pads */
 301 static int imx_media_create_pad_vdev_lists(struct imx_media_dev *imxmd)
 302 {
 303         struct imx_media_video_dev *vdev;
 304         struct media_link *link;
 305         int i, ret;
 306
 307         for (i = 0; i < imxmd->num_vdevs; i++) {
 308                 vdev = imxmd->vdev[i];
 309                 link = list_first_entry(&vdev->vfd->entity.links,
 310                                         struct media_link, list);
 311                 ret = imx_media_add_vdev_to_pad(imxmd, vdev, link->source);
 312                 if (ret)
 313                         return ret;
 314         }
 315
 316         return 0;
 317 }
 318
 319 /* async subdev complete notifier */
 320 static int imx_media_probe_complete(struct v4l2_async_notifier *notifier)
 321 {
 322         struct imx_media_dev *imxmd = notifier2dev(notifier);
 323         int i, ret;
 324
 325         mutex_lock(&imxmd->mutex);
 326
 327         /* make sure all subdevs were bound */
 328         for (i = 0; i < imxmd->num_subdevs; i++) {
 329                 if (!imxmd->subdev[i].sd) {
 330                         v4l2_err(&imxmd->v4l2_dev, "unbound subdev!\n");
 331                         ret = -ENODEV;
 332                         goto unlock;
 333                 }
 334         }
 335
 336         ret = imx_media_create_links(imxmd);
 337         if (ret)
 338                 goto unlock;
 339
 340         ret = imx_media_create_pad_vdev_lists(imxmd);
 341         if (ret)
 342                 goto unlock;
 343
 344         ret = v4l2_device_register_subdev_nodes(&imxmd->v4l2_dev);
 345 unlock:
 346         mutex_unlock(&imxmd->mutex);
 347         if (ret)
 348                 return ret;
 349
 350         return media_device_register(&imxmd->md);
 351 }
 352
 353 static const struct v4l2_async_notifier_operations imx_media_subdev_ops = {
 354         .bound = imx_media_subdev_bound,
 355         .complete = imx_media_probe_complete,
 356 };
 357
 358 /*
 359  * adds controls to a video device from an entity subdevice.
 360  * Continues upstream from the entity's sink pads.
 361  */
 362 static int imx_media_inherit_controls(struct imx_media_dev *imxmd,
 363                                       struct video_device *vfd,
 364                                       struct media_entity *entity)
 365 {
 366         int i, ret = 0;
 367
 368         if (is_media_entity_v4l2_subdev(entity)) {
 369                 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 370
 371                 dev_dbg(imxmd->md.dev,
 372                         "adding controls to %s from %s\n",
 373                         vfd->entity.name, sd->entity.name);
 374
 375                 ret = v4l2_ctrl_add_handler(vfd->ctrl_handler,
 376                                             sd->ctrl_handler,
 377                                             NULL);
 378                 if (ret)
 379                         return ret;
 380         }
 381
 382         /* move upstream */
 383         for (i = 0; i < entity->num_pads; i++) {
 384                 struct media_pad *pad, *spad = &entity->pads[i];
 385
 386                 if (!(spad->flags & MEDIA_PAD_FL_SINK))
 387                         continue;
 388
 389                 pad = media_entity_remote_pad(spad);
 390                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 391                         continue;
 392
 393                 ret = imx_media_inherit_controls(imxmd, vfd, pad->entity);
 394                 if (ret)
 395                         break;
 396         }
 397
 398         return ret;
 399 }
 400
 401 static int imx_media_link_notify(struct media_link *link, u32 flags,
 402                                  unsigned int notification)
 403 {
 404         struct media_entity *source = link->source->entity;
 405         struct imx_media_subdev *imxsd;
 406         struct imx_media_pad *imxpad;
 407         struct imx_media_dev *imxmd;
 408         struct video_device *vfd;
 409         struct v4l2_subdev *sd;
 410         int i, pad_idx, ret;
 411
 412         ret = v4l2_pipeline_link_notify(link, flags, notification);
 413         if (ret)
 414                 return ret;
 415
 416         /* don't bother if source is not a subdev */
 417         if (!is_media_entity_v4l2_subdev(source))
 418                 return 0;
 419
 420         sd = media_entity_to_v4l2_subdev(source);
 421         pad_idx = link->source->index;
 422
 423         imxmd = dev_get_drvdata(sd->v4l2_dev->dev);
 424
 425         imxsd = imx_media_find_subdev_by_sd(imxmd, sd);
 426         if (IS_ERR(imxsd))
 427                 return PTR_ERR(imxsd);
 428         imxpad = &imxsd->pad[pad_idx];
 429
 430         /*
 431          * Before disabling a link, reset controls for all video
 432          * devices reachable from this link.
 433          *
 434          * After enabling a link, refresh controls for all video
 435          * devices reachable from this link.
 436          */
 437         if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
 438             !(flags & MEDIA_LNK_FL_ENABLED)) {
 439                 for (i = 0; i < imxpad->num_vdevs; i++) {
 440                         vfd = imxpad->vdev[i]->vfd;
 441                         dev_dbg(imxmd->md.dev,
 442                                 "reset controls for %s\n",
 443                                 vfd->entity.name);
 444                         v4l2_ctrl_handler_free(vfd->ctrl_handler);
 445                         v4l2_ctrl_handler_init(vfd->ctrl_handler, 0);
 446                 }
 447         } else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
 448                    (link->flags & MEDIA_LNK_FL_ENABLED)) {
 449                 for (i = 0; i < imxpad->num_vdevs; i++) {
 450                         vfd = imxpad->vdev[i]->vfd;
 451                         dev_dbg(imxmd->md.dev,
 452                                 "refresh controls for %s\n",
 453                                 vfd->entity.name);
 454                         ret = imx_media_inherit_controls(imxmd, vfd,
 455                                                          &vfd->entity);
 456                         if (ret)
 457                                 break;
 458                 }
 459         }
 460
 461         return ret;
 462 }
 463
 464 static const struct media_device_ops imx_media_md_ops = {
 465         .link_notify = imx_media_link_notify,
 466 };
 467
 468 static int imx_media_probe(struct platform_device *pdev)
 469 {
 470         struct device *dev = &pdev->dev;
 471         struct device_node *node = dev->of_node;
 472         struct imx_media_dev *imxmd;
 473         int ret;
 474
 475         imxmd = devm_kzalloc(dev, sizeof(*imxmd), GFP_KERNEL);
 476         if (!imxmd)
 477                 return -ENOMEM;
 478
 479         dev_set_drvdata(dev, imxmd);
 480
 481         strlcpy(imxmd->md.model, "imx-media", sizeof(imxmd->md.model));
 482         imxmd->md.ops = &imx_media_md_ops;
 483         imxmd->md.dev = dev;
 484
 485         mutex_init(&imxmd->mutex);
 486
 487         imxmd->v4l2_dev.mdev = &imxmd->md;
 488         strlcpy(imxmd->v4l2_dev.name, "imx-media",
 489                 sizeof(imxmd->v4l2_dev.name));
 490
 491         media_device_init(&imxmd->md);
 492
 493         ret = v4l2_device_register(dev, &imxmd->v4l2_dev);
 494         if (ret < 0) {
 495                 v4l2_err(&imxmd->v4l2_dev,
 496                          "Failed to register v4l2_device: %d\n", ret);
 497                 goto cleanup;
 498         }
 499
 500         dev_set_drvdata(imxmd->v4l2_dev.dev, imxmd);
 501
 502         ret = imx_media_add_of_subdevs(imxmd, node);
 503         if (ret) {
 504                 v4l2_err(&imxmd->v4l2_dev,
 505                          "add_of_subdevs failed with %d\n", ret);
 506                 goto unreg_dev;
 507         }
 508
 509         ret = imx_media_add_internal_subdevs(imxmd);
 510         if (ret) {
 511                 v4l2_err(&imxmd->v4l2_dev,
 512                          "add_internal_subdevs failed with %d\n", ret);
 513                 goto unreg_dev;
 514         }
 515
 516         /* no subdevs? just bail */
 517         imxmd->num_subdevs = imxmd->subdev_notifier.num_subdevs;
 518         if (imxmd->num_subdevs == 0) {
 519                 ret = -ENODEV;
 520                 goto unreg_dev;
 521         }
 522
 523         /* prepare the async subdev notifier and register it */
 524         imxmd->subdev_notifier.subdevs = imxmd->async_ptrs;
 525         imxmd->subdev_notifier.ops = &imx_media_subdev_ops;
 526         ret = v4l2_async_notifier_register(&imxmd->v4l2_dev,
 527                                            &imxmd->subdev_notifier);
 528         if (ret) {
 529                 v4l2_err(&imxmd->v4l2_dev,
 530                          "v4l2_async_notifier_register failed with %d\n", ret);
 531                 goto del_int;
 532         }
 533
 534         return 0;
 535
 536 del_int:
 537         imx_media_remove_internal_subdevs(imxmd);
 538 unreg_dev:
 539         v4l2_device_unregister(&imxmd->v4l2_dev);
 540 cleanup:
 541         media_device_cleanup(&imxmd->md);
 542         return ret;
 543 }
 544
 545 static int imx_media_remove(struct platform_device *pdev)
 546 {
 547         struct imx_media_dev *imxmd =
 548                 (struct imx_media_dev *)platform_get_drvdata(pdev);
 549
 550         v4l2_info(&imxmd->v4l2_dev, "Removing imx-media\n");
 551
 552         v4l2_async_notifier_unregister(&imxmd->subdev_notifier);
 553         imx_media_remove_internal_subdevs(imxmd);
 554         v4l2_device_unregister(&imxmd->v4l2_dev);
 555         media_device_unregister(&imxmd->md);
 556         media_device_cleanup(&imxmd->md);
 557
 558         return 0;
 559 }
 560
 561 static const struct of_device_id imx_media_dt_ids[] = {
 562         { .compatible = "fsl,imx-capture-subsystem" },
 563         { /* sentinel */ }
 564 };
 565 MODULE_DEVICE_TABLE(of, imx_media_dt_ids);
 566
 567 static struct platform_driver imx_media_pdrv = {
 568         .probe          = imx_media_probe,
 569         .remove         = imx_media_remove,
 570         .driver         = {
 571                 .name   = "imx-media",
 572                 .of_match_table = imx_media_dt_ids,
 573         },
 574 };
 575
 576 module_platform_driver(imx_media_pdrv);
 577
 578 MODULE_DESCRIPTION("i.MX5/6 v4l2 media controller driver");
 579 MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>");
 580 MODULE_LICENSE("GPL");