src/mediactl.c

   1 /*
   2  * Media controller interface library
   3  *
   4  * Copyright (C) 2010 Ideas on board SPRL <laurent.pinchart@ideasonboard.com>
   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  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License along
  17  * with this program; if not, write to the Free Software Foundation, Inc.,
  18  */
  19
  20 #include "config.h"
  21
  22 #include <sys/ioctl.h>
  23 #include <sys/stat.h>
  24 #include <sys/types.h>
  25
  26 #include <unistd.h>
  27 #include <stdio.h>
  28 #include <stdlib.h>
  29 #include <string.h>
  30 #include <fcntl.h>
  31 #include <errno.h>
  32
  33 #include <linux/videodev2.h>
  34 #include <linux/media.h>
  35
  36 #include "mediactl.h"
  37 #include "tools.h"
  38
  39 struct media_pad *media_entity_remote_source(struct media_pad *pad)
  40 {
  41         unsigned int i;
  42
  43         if (!(pad->flags & MEDIA_PAD_FL_SINK))
  44                 return NULL;
  45
  46         for (i = 0; i < pad->entity->num_links; ++i) {
  47                 struct media_link *link = &pad->entity->links[i];
  48
  49                 if (!(link->flags & MEDIA_LNK_FL_ENABLED))
  50                         continue;
  51
  52                 if (link->sink == pad)
  53                         return link->source;
  54         }
  55
  56         return NULL;
  57 }
  58
  59 struct media_entity *media_get_entity_by_name(struct media_device *media,
  60                                               const char *name, size_t length)
  61 {
  62         unsigned int i;
  63
  64         for (i = 0; i < media->entities_count; ++i) {
  65                 struct media_entity *entity = &media->entities[i];
  66
  67                 if (strncmp(entity->info.name, name, length) == 0)
  68                         return entity;
  69         }
  70
  71         return NULL;
  72 }
  73
  74 struct media_entity *media_get_entity_by_id(struct media_device *media,
  75                                             __u32 id)
  76 {
  77         unsigned int i;
  78
  79         for (i = 0; i < media->entities_count; ++i) {
  80                 struct media_entity *entity = &media->entities[i];
  81
  82                 if (entity->info.id == id)
  83                         return entity;
  84         }
  85
  86         return NULL;
  87 }
  88
  89 int media_setup_link(struct media_device *media,
  90                      struct media_pad *source,
  91                      struct media_pad *sink,
  92                      __u32 flags)
  93 {
  94         struct media_link *link;
  95         struct media_link_desc ulink;
  96         unsigned int i;
  97         int ret;
  98
  99         for (i = 0; i < source->entity->num_links; i++) {
 100                 link = &source->entity->links[i];
 101
 102                 if (link->source->entity == source->entity &&
 103                     link->source->index == source->index &&
 104                     link->sink->entity == sink->entity &&
 105                     link->sink->index == sink->index)
 106                         break;
 107         }
 108
 109         if (i == source->entity->num_links) {
 110                 media_dbg(media, "%s: Link not found\n", __func__);
 111                 return -ENOENT;
 112         }
 113
 114         /* source pad */
 115         ulink.source.entity = source->entity->info.id;
 116         ulink.source.index = source->index;
 117         ulink.source.flags = MEDIA_PAD_FL_SOURCE;
 118
 119         /* sink pad */
 120         ulink.sink.entity = sink->entity->info.id;
 121         ulink.sink.index = sink->index;
 122         ulink.sink.flags = MEDIA_PAD_FL_SINK;
 123
 124         ulink.flags = flags | (link->flags & MEDIA_LNK_FL_IMMUTABLE);
 125
 126         ret = ioctl(media->fd, MEDIA_IOC_SETUP_LINK, &ulink);
 127         if (ret == -1) {
 128                 media_dbg(media, "%s: Unable to setup link (%s)\n",
 129                           __func__, strerror(errno));
 130                 return -errno;
 131         }
 132
 133         link->flags = ulink.flags;
 134         link->twin->flags = ulink.flags;
 135         return 0;
 136 }
 137
 138 int media_reset_links(struct media_device *media)
 139 {
 140         unsigned int i, j;
 141         int ret;
 142
 143         for (i = 0; i < media->entities_count; ++i) {
 144                 struct media_entity *entity = &media->entities[i];
 145
 146                 for (j = 0; j < entity->num_links; j++) {
 147                         struct media_link *link = &entity->links[j];
 148
 149                         if (link->flags & MEDIA_LNK_FL_IMMUTABLE ||
 150                             link->source->entity != entity)
 151                                 continue;
 152
 153                         ret = media_setup_link(media, link->source, link->sink,
 154                                                link->flags & ~MEDIA_LNK_FL_ENABLED);
 155                         if (ret < 0)
 156                                 return ret;
 157                 }
 158         }
 159
 160         return 0;
 161 }
 162
 163 static struct media_link *media_entity_add_link(struct media_entity *entity)
 164 {
 165         if (entity->num_links >= entity->max_links) {
 166                 struct media_link *links = entity->links;
 167                 unsigned int max_links = entity->max_links * 2;
 168                 unsigned int i;
 169
 170                 links = realloc(links, max_links * sizeof *links);
 171                 if (links == NULL)
 172                         return NULL;
 173
 174                 for (i = 0; i < entity->num_links; ++i)
 175                         links[i].twin->twin = &links[i];
 176
 177                 entity->max_links = max_links;
 178                 entity->links = links;
 179         }
 180
 181         return &entity->links[entity->num_links++];
 182 }
 183
 184 static int media_enum_links(struct media_device *media)
 185 {
 186         __u32 id;
 187         int ret = 0;
 188
 189         for (id = 1; id <= media->entities_count; id++) {
 190                 struct media_entity *entity = &media->entities[id - 1];
 191                 struct media_links_enum links;
 192                 unsigned int i;
 193
 194                 links.entity = entity->info.id;
 195                 links.pads = malloc(entity->info.pads * sizeof(struct media_pad_desc));
 196                 links.links = malloc(entity->info.links * sizeof(struct media_link_desc));
 197
 198                 if (ioctl(media->fd, MEDIA_IOC_ENUM_LINKS, &links) < 0) {
 199                         media_dbg(media,
 200                                   "%s: Unable to enumerate pads and links (%s).\n",
 201                                   __func__, strerror(errno));
 202                         free(links.pads);
 203                         free(links.links);
 204                         return -errno;
 205                 }
 206
 207                 for (i = 0; i < entity->info.pads; ++i) {
 208                         entity->pads[i].entity = entity;
 209                         entity->pads[i].index = links.pads[i].index;
 210                         entity->pads[i].flags = links.pads[i].flags;
 211                 }
 212
 213                 for (i = 0; i < entity->info.links; ++i) {
 214                         struct media_link_desc *link = &links.links[i];
 215                         struct media_link *fwdlink;
 216                         struct media_link *backlink;
 217                         struct media_entity *source;
 218                         struct media_entity *sink;
 219
 220                         source = media_get_entity_by_id(media, link->source.entity);
 221                         sink = media_get_entity_by_id(media, link->sink.entity);
 222
 223                         if (source == NULL || sink == NULL) {
 224                                 media_dbg(media,
 225                                           "WARNING entity %u link %u from %u/%u to %u/%u is invalid!\n",
 226                                           id, i, link->source.entity,
 227                                           link->source.index,
 228                                           link->sink.entity,
 229                                           link->sink.index);
 230                                 ret = -EINVAL;
 231                         } else {
 232                                 fwdlink = media_entity_add_link(source);
 233                                 fwdlink->source = &source->pads[link->source.index];
 234                                 fwdlink->sink = &sink->pads[link->sink.index];
 235                                 fwdlink->flags = link->flags;
 236
 237                                 backlink = media_entity_add_link(sink);
 238                                 backlink->source = &source->pads[link->source.index];
 239                                 backlink->sink = &sink->pads[link->sink.index];
 240                                 backlink->flags = link->flags;
 241
 242                                 fwdlink->twin = backlink;
 243                                 backlink->twin = fwdlink;
 244                         }
 245                 }
 246
 247                 free(links.pads);
 248                 free(links.links);
 249         }
 250
 251         return ret;
 252 }
 253
 254 #ifdef HAVE_LIBUDEV
 255
 256 #include <libudev.h>
 257
 258 static inline int media_udev_open(struct udev **udev)
 259 {
 260         *udev = udev_new();
 261         if (*udev == NULL)
 262                 return -ENOMEM;
 263         return 0;
 264 }
 265
 266 static inline void media_udev_close(struct udev *udev)
 267 {
 268         if (udev != NULL)
 269                 udev_unref(udev);
 270 }
 271
 272 static int media_get_devname_udev(struct udev *udev,
 273                 struct media_entity *entity, int verbose)
 274 {
 275         struct udev_device *device;
 276         dev_t devnum;
 277         const char *p;
 278         int ret = -ENODEV;
 279
 280         if (udev == NULL)
 281                 return -EINVAL;
 282
 283         devnum = makedev(entity->info.v4l.major, entity->info.v4l.minor);
 284         if (verbose)
 285                 media_dbg(entity->media, "looking up device: %u:%u\n",
 286                           major(devnum), minor(devnum));
 287         device = udev_device_new_from_devnum(udev, 'c', devnum);
 288         if (device) {
 289                 p = udev_device_get_devnode(device);
 290                 if (p) {
 291                         strncpy(entity->devname, p, sizeof(entity->devname));
 292                         entity->devname[sizeof(entity->devname) - 1] = '\0';
 293                 }
 294                 ret = 0;
 295         }
 296
 297         udev_device_unref(device);
 298
 299         return ret;
 300 }
 301
 302 #else   /* HAVE_LIBUDEV */
 303
 304 struct udev;
 305
 306 static inline int media_udev_open(struct udev **udev) { return 0; }
 307
 308 static inline void media_udev_close(struct udev *udev) { }
 309
 310 static inline int media_get_devname_udev(struct udev *udev,
 311                 struct media_entity *entity, int verbose)
 312 {
 313         return -ENOTSUP;
 314 }
 315
 316 #endif  /* HAVE_LIBUDEV */
 317
 318 static int media_get_devname_sysfs(struct media_entity *entity)
 319 {
 320         struct stat devstat;
 321         char devname[32];
 322         char sysname[32];
 323         char target[1024];
 324         char *p;
 325         int ret;
 326
 327         sprintf(sysname, "/sys/dev/char/%u:%u", entity->info.v4l.major,
 328                 entity->info.v4l.minor);
 329         ret = readlink(sysname, target, sizeof(target));
 330         if (ret < 0)
 331                 return -errno;
 332
 333         target[ret] = '\0';
 334         p = strrchr(target, '/');
 335         if (p == NULL)
 336                 return -EINVAL;
 337
 338         sprintf(devname, "/dev/%s", p + 1);
 339         ret = stat(devname, &devstat);
 340         if (ret < 0)
 341                 return -errno;
 342
 343         /* Sanity check: udev might have reordered the device nodes.
 344          * Make sure the major/minor match. We should really use
 345          * libudev.
 346          */
 347         if (major(devstat.st_rdev) == entity->info.v4l.major &&
 348             minor(devstat.st_rdev) == entity->info.v4l.minor)
 349                 strcpy(entity->devname, devname);
 350
 351         return 0;
 352 }
 353
 354 static int media_enum_entities(struct media_device *media, int verbose)
 355 {
 356         struct media_entity *entity;
 357         struct udev *udev;
 358         unsigned int size;
 359         __u32 id;
 360         int ret;
 361
 362         ret = media_udev_open(&udev);
 363         if (ret < 0)
 364                 media_dbg(media, "Can't get udev context\n");
 365
 366         for (id = 0, ret = 0; ; id = entity->info.id) {
 367                 size = (media->entities_count + 1) * sizeof(*media->entities);
 368                 media->entities = realloc(media->entities, size);
 369
 370                 entity = &media->entities[media->entities_count];
 371                 memset(entity, 0, sizeof(*entity));
 372                 entity->fd = -1;
 373                 entity->info.id = id | MEDIA_ENT_ID_FLAG_NEXT;
 374                 entity->media = media;
 375
 376                 ret = ioctl(media->fd, MEDIA_IOC_ENUM_ENTITIES, &entity->info);
 377                 if (ret < 0) {
 378                         ret = errno != EINVAL ? -errno : 0;
 379                         break;
 380                 }
 381
 382                 /* Number of links (for outbound links) plus number of pads (for
 383                  * inbound links) is a good safe initial estimate of the total
 384                  * number of links.
 385                  */
 386                 entity->max_links = entity->info.pads + entity->info.links;
 387
 388                 entity->pads = malloc(entity->info.pads * sizeof(*entity->pads));
 389                 entity->links = malloc(entity->max_links * sizeof(*entity->links));
 390                 if (entity->pads == NULL || entity->links == NULL) {
 391                         ret = -ENOMEM;
 392                         break;
 393                 }
 394
 395                 media->entities_count++;
 396
 397                 /* Find the corresponding device name. */
 398                 if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE &&
 399                     media_entity_type(entity) != MEDIA_ENT_T_V4L2_SUBDEV)
 400                         continue;
 401
 402                 /* Try to get the device name via udev */
 403                 if (!media_get_devname_udev(udev, entity, verbose))
 404                         continue;
 405
 406                 /* Fall back to get the device name via sysfs */
 407                 media_get_devname_sysfs(entity);
 408         }
 409
 410         media_udev_close(udev);
 411         return ret;
 412 }
 413
 414 static void media_debug_default(void *ptr, ...)
 415 {
 416 }
 417
 418 void media_debug_set_handler(struct media_device *media,
 419                              void (*debug_handler)(void *, ...),
 420                              void *debug_priv)
 421 {
 422         if (debug_handler) {
 423                 media->debug_handler = debug_handler;
 424                 media->debug_priv = debug_priv;
 425         } else {
 426                 media->debug_handler = media_debug_default;
 427                 media->debug_priv = NULL;
 428         }
 429 }
 430
 431 struct media_device *media_open_debug(
 432         const char *name, int verbose, void (*debug_handler)(void *, ...),
 433         void *debug_priv)
 434 {
 435         struct media_device *media;
 436         int ret;
 437
 438         media = calloc(1, sizeof(*media));
 439         if (media == NULL)
 440                 return NULL;
 441
 442         media_debug_set_handler(media, debug_handler, debug_priv);
 443
 444         media_dbg(media, "Opening media device %s\n", name);
 445
 446         media->fd = open(name, O_RDWR);
 447         if (media->fd < 0) {
 448                 media_close(media);
 449                 media_dbg(media, "%s: Can't open media device %s\n",
 450                           __func__, name);
 451                 return NULL;
 452         }
 453
 454         media_dbg(media, "Enumerating entities\n");
 455
 456         ret = media_enum_entities(media, verbose);
 457
 458         if (ret < 0) {
 459                 media_dbg(media,
 460                           "%s: Unable to enumerate entities for device %s (%s)\n",
 461                           __func__, name, strerror(-ret));
 462                 media_close(media);
 463                 return NULL;
 464         }
 465
 466         media_dbg(media, "Found %u entities\n", media->entities_count);
 467         media_dbg(media, "Enumerating pads and links\n");
 468
 469         ret = media_enum_links(media);
 470         if (ret < 0) {
 471                 media_dbg(media,
 472                           "%s: Unable to enumerate pads and linksfor device %s\n",
 473                           __func__, name);
 474                 media_close(media);
 475                 return NULL;
 476         }
 477
 478         return media;
 479 }
 480
 481 struct media_device *media_open(const char *name, int verbose)
 482 {
 483         return media_open_debug(name, verbose, NULL, NULL);
 484 }
 485
 486 void media_close(struct media_device *media)
 487 {
 488         unsigned int i;
 489
 490         if (media->fd != -1)
 491                 close(media->fd);
 492
 493         for (i = 0; i < media->entities_count; ++i) {
 494                 struct media_entity *entity = &media->entities[i];
 495
 496                 free(entity->pads);
 497                 free(entity->links);
 498                 if (entity->fd != -1)
 499                         close(entity->fd);
 500         }
 501
 502         free(media->entities);
 503         free(media);
 504 }
 505
 506 struct media_pad *media_parse_pad(struct media_device *media,
 507                                   const char *p, char **endp)
 508 {
 509         unsigned int entity_id, pad;
 510         struct media_entity *entity;
 511         char *end;
 512
 513         for (; isspace(*p); ++p);
 514
 515         if (*p == '"') {
 516                 for (end = (char *)p + 1; *end && *end != '"'; ++end);
 517                 if (*end != '"')
 518                         return NULL;
 519
 520                 entity = media_get_entity_by_name(media, p + 1, end - p - 1);
 521                 if (entity == NULL)
 522                         return NULL;
 523
 524                 ++end;
 525         } else {
 526                 entity_id = strtoul(p, &end, 10);
 527                 entity = media_get_entity_by_id(media, entity_id);
 528                 if (entity == NULL)
 529                         return NULL;
 530         }
 531         for (; isspace(*end); ++end);
 532
 533         if (*end != ':')
 534                 return NULL;
 535         for (p = end + 1; isspace(*p); ++p);
 536
 537         pad = strtoul(p, &end, 10);
 538         for (p = end; isspace(*p); ++p);
 539
 540         if (pad >= entity->info.pads)
 541                 return NULL;
 542
 543         for (p = end; isspace(*p); ++p);
 544         if (endp)
 545                 *endp = (char *)p;
 546
 547         return &entity->pads[pad];
 548 }
 549
 550 struct media_link *media_parse_link(struct media_device *media,
 551                                     const char *p, char **endp)
 552 {
 553         struct media_link *link;
 554         struct media_pad *source;
 555         struct media_pad *sink;
 556         unsigned int i;
 557         char *end;
 558
 559         source = media_parse_pad(media, p, &end);
 560         if (source == NULL)
 561                 return NULL;
 562
 563         if (end[0] != '-' || end[1] != '>')
 564                 return NULL;
 565         p = end + 2;
 566
 567         sink = media_parse_pad(media, p, &end);
 568         if (sink == NULL)
 569                 return NULL;
 570
 571         *endp = end;
 572
 573         for (i = 0; i < source->entity->num_links; i++) {
 574                 link = &source->entity->links[i];
 575
 576                 if (link->source == source && link->sink == sink)
 577                         return link;
 578         }
 579
 580         return NULL;
 581 }
 582
 583 int media_parse_setup_link(struct media_device *media,
 584                            const char *p, char **endp)
 585 {
 586         struct media_link *link;
 587         __u32 flags;
 588         char *end;
 589
 590         link = media_parse_link(media, p, &end);
 591         if (link == NULL) {
 592                 media_dbg(media,
 593                           "%s: Unable to parse link\n", __func__);
 594                 return -EINVAL;
 595         }
 596
 597         p = end;
 598         if (*p++ != '[') {
 599                 media_dbg(media, "Unable to parse link flags\n");
 600                 return -EINVAL;
 601         }
 602
 603         flags = strtoul(p, &end, 10);
 604         for (p = end; isspace(*p); p++);
 605         if (*p++ != ']') {
 606                 media_dbg(media, "Unable to parse link flags\n");
 607                 return -EINVAL;
 608         }
 609
 610         for (; isspace(*p); p++);
 611         *endp = (char *)p;
 612
 613         media_dbg(media,
 614                   "Setting up link %u:%u -> %u:%u [%u]\n",
 615                   link->source->entity->info.id, link->source->index,
 616                   link->sink->entity->info.id, link->sink->index,
 617                   flags);
 618
 619         return media_setup_link(media, link->source, link->sink, flags);
 620 }
 621
 622 int media_parse_setup_links(struct media_device *media, const char *p)
 623 {
 624         char *end;
 625         int ret;
 626
 627         do {
 628                 ret = media_parse_setup_link(media, p, &end);
 629                 if (ret < 0)
 630                         return ret;
 631
 632                 p = end + 1;
 633         } while (*end == ',');
 634
 635         return *end ? -EINVAL : 0;
 636 }