src/mediactl.c

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