util/v4l2.c

   1 /*
   2  * Copyright (C) 2011 Texas Instruments
   3  * Author: Rob Clark <rob.clark@linaro.org>
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of the GNU General Public License version 2 as published by
   7  * the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #ifdef HAVE_CONFIG_H
  19 #include "config.h"
  20 #endif
  21
  22 #include <linux/videodev2.h>
  23 #include <sys/types.h>
  24 #include <sys/stat.h>
  25 #include <fcntl.h>
  26 #include <sys/ioctl.h>
  27
  28 #include "util.h"
  29
  30 struct v4l2 {
  31         int fd;
  32         int nbufs;
  33         struct v4l2_buffer *v4l2bufs;
  34         struct buffer **bufs;
  35 };
  36
  37 void
  38 v4l2_usage(void)
  39 {
  40         MSG("V4L2 Capture Options:");
  41         MSG("\t-c WxH@fourcc\tset capture dimensions/format");
  42         MSG("\t-m\t\tdo MCF setup");
  43 }
  44
  45 /* media_* helpers to do the MCF dance to get things configured properly
  46  * so that we can set the specified format on the output device.  For
  47  * non-MCF cameras this can just be skipped.
  48  */
  49 #include <linux/media.h>
  50 #include <linux/v4l2-subdev.h>
  51
  52 static int
  53 media_open_entity(struct media_entity_desc *entity)
  54 {
  55         struct stat devstat;
  56         char devname[32];
  57         char sysname[32];
  58         char target[1024];
  59         char *p;
  60         int ret;
  61
  62         sprintf(sysname, "/sys/dev/char/%u:%u", entity->v4l.major,
  63                         entity->v4l.minor);
  64         ret = readlink(sysname, target, sizeof(target));
  65         if (ret < 0)
  66                 return -errno;
  67
  68
  69         target[ret] = '\0';
  70         p = strrchr(target, '/');
  71         if (p == NULL)
  72                 return -EINVAL;
  73
  74         sprintf(devname, "/dev/%s", p + 1);
  75 MSG("\t%s -> %s -> %s", sysname, target, devname);
  76         ret = stat(devname, &devstat);
  77         if (ret < 0)
  78                 return -errno;
  79
  80         /* Sanity check: udev might have reordered the device nodes.
  81          * Make sure the major/minor match. We should really use
  82          * libudev.
  83          */
  84         if (major(devstat.st_rdev) == entity->v4l.major &&
  85             minor(devstat.st_rdev) == entity->v4l.minor) {
  86                 return open(devname, O_RDWR);
  87         }
  88
  89         return -1;
  90 }
  91
  92 static int
  93 media_find_entity(int fd, struct media_entity_desc *entity,
  94                 uint32_t type, uint32_t entity_id)
  95 {
  96         int id, ret = 0;
  97
  98         for (id = 0; ; id = entity->id) {
  99                 memset(entity, 0, sizeof(*entity));
 100                 entity->id = id | MEDIA_ENT_ID_FLAG_NEXT;
 101
 102                 ret = ioctl(fd, MEDIA_IOC_ENUM_ENTITIES, entity);
 103                 if (ret) {
 104                         ERROR("MEDIA_IOC_ENUM_ENTITIES failed: %s (%d)",
 105                                         strerror(errno), ret);
 106                         break;
 107                 }
 108
 109 MSG("\tfound entity: %s type=%08x, flags=%08x, group_id=%d, pads=%d, links=%d",
 110 entity->name, entity->type, entity->flags, entity->group_id,
 111 entity->pads, entity->links);
 112
 113                 if ((entity->type == type) || (entity->id == entity_id)) {
 114                         return 0;
 115                 }
 116         }
 117
 118         return ret;
 119 }
 120
 121 static void
 122 media_configure(struct media_entity_desc *entity,
 123                 struct v4l2_format *format, int pad)
 124 {
 125         struct v4l2_subdev_format ent_format = {
 126                         .pad = pad,
 127                         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 128                         .format = {
 129                                         .width  = format->fmt.pix.width,
 130                                         .height = format->fmt.pix.height,
 131                                         .code   = V4L2_MBUS_FMT_UYVY8_1X16,
 132                                         .field  = V4L2_FIELD_NONE,
 133                                         .colorspace = V4L2_COLORSPACE_JPEG,
 134                         },
 135         };
 136         int fd, ret;
 137
 138         fd = media_open_entity(entity);
 139         if (fd < 0) {
 140                 ERROR("could not open media device: \"%s\" (%d:%d)", entity->name,
 141                                 entity->v4l.major, entity->v4l.minor);
 142                 return;
 143         }
 144
 145         MSG("Setting format for: \"%s\" (%d)", entity->name, pad);
 146         ret = ioctl(fd, VIDIOC_SUBDEV_S_FMT, &ent_format);
 147         if (ret) {
 148                 MSG("Could not configure: %s (%d)", strerror(errno), ret);
 149         }
 150 }
 151
 152 /* walk the graph and attempt to configure all the nodes to the same settings.
 153  * This works for smart-sensor with no element in between that can convert/
 154  * scale..  If the sensor can't handle the settings, then S_FMT just fails
 155  * and hopefully some element in between can pick up the slack.
 156  */
 157 static int
 158 media_setup(struct v4l2_format *format)
 159 {
 160         struct media_entity_desc entity;
 161         int fd, ret;
 162
 163         fd = open("/dev/media0", O_RDWR);
 164         if (fd < 0) {
 165                 ERROR("could not open MCF: %s (%d)", strerror(errno), ret);
 166                 return fd;
 167         }
 168
 169         ret = media_find_entity(fd, &entity, MEDIA_ENT_T_V4L2_SUBDEV_SENSOR, ~0);
 170         if (ret) {
 171                 return ret;
 172         }
 173
 174         /* now walk the graph to the output, configure everything on the way: */
 175         do {
 176                 struct media_link_desc links[10];
 177                 struct media_links_enum link_enum = {
 178                                 .entity = entity.id,
 179                                 .links = links,
 180                 };
 181                 int i;
 182
 183                 ret = ioctl(fd, MEDIA_IOC_ENUM_LINKS, &link_enum);
 184                 if (ret) {
 185                         ERROR("MEDIA_IOC_ENUM_LINKS failed: %s (%d)",
 186                                         strerror(errno), ret);
 187                         return ret;
 188                 }
 189
 190                 for (i = 0; i < entity.links; i++) {
 191                         if (links[i].source.entity == entity.id) {
 192                                 // XXX maybe if there are multiple links, we should prefer
 193                                 // an enabled link, otherwise just pick one..
 194
 195                                 media_configure(&entity, format, links[i].source.index);
 196                                 media_configure(&entity, format, links[i].sink.index);
 197
 198                                 /* lets take this link.. */
 199                                 if (!(links[i].flags & MEDIA_LNK_FL_ENABLED)) {
 200                                         links[i].flags |= MEDIA_LNK_FL_ENABLED;
 201                                         ret = ioctl(fd, MEDIA_IOC_SETUP_LINK, &links[i]);
 202                                         if (ret) {
 203                                                 ERROR("MEDIA_IOC_SETUP_LINK failed: %s (%d)",
 204                                                                 strerror(errno), errno);
 205 //                                              return ret;
 206                                         }
 207                                 }
 208
 209                                 ret = media_find_entity(fd, &entity, ~0, links[i].sink.entity);
 210                                 if (ret) {
 211                                         return ret;
 212                                 }
 213
 214                                 break;
 215                         }
 216                 }
 217         } while (entity.type != MEDIA_ENT_T_DEVNODE_V4L);
 218
 219         return 0;
 220 }
 221
 222 /* Open v4l2 (and media0??) XXX */
 223 struct v4l2 *
 224 v4l2_open(int argc, char **argv, uint32_t *fourcc,
 225                 uint32_t *width, uint32_t *height)
 226 {
 227         struct v4l2_format format = {
 228                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
 229         };
 230         struct v4l2 *v4l2;
 231         int i, ret;
 232         bool mcf = false, user_format = false, user_dev = false;
 233         char devname[20] = "/dev/video1";
 234
 235         v4l2 = calloc(1, sizeof(*v4l2));
 236
 237         /* note: set args to NULL after we've parsed them so other modules know
 238          * that it is already parsed (since the arg parsing is decentralized)
 239          */
 240         for (i = 1; i < argc; i++) {
 241                 if (!argv[i]) {
 242                         continue;
 243                 }
 244                 if (!strcmp("-d", argv[i])) {
 245                         if(user_dev)
 246                                 continue;
 247
 248                         argv[i++] = NULL;
 249                         if (sscanf(argv[i], "%s", devname) != 1) {
 250                                 ERROR("invalid device node: %s", argv[i]);
 251                                 goto fail;
 252                         }
 253                         user_dev = true;
 254                 } else if (!strcmp("-c", argv[i])) {
 255                         if(user_format)
 256                                 continue;
 257
 258                         char fourccstr[5];
 259                         argv[i++] = NULL;
 260                         if (sscanf(argv[i], "%ux%u@%4s",
 261                                         width,
 262                                         height,
 263                                         fourccstr) != 3) {
 264                                 ERROR("invalid arg: %s", argv[i]);
 265                                 goto fail;
 266                         }
 267                         *fourcc = FOURCC_STR(fourccstr);
 268                         user_format = true;
 269                 } else if (!strcmp(argv[i], "-m")) {
 270                         mcf = true;
 271                 } else {
 272                         continue;
 273                 }
 274                 argv[i] = NULL;
 275         }
 276
 277         v4l2->fd = open(devname, O_RDWR);
 278
 279         ret = ioctl(v4l2->fd, VIDIOC_G_FMT, &format);
 280         if (ret < 0) {
 281                 ERROR("VIDIOC_G_FMT failed: %s (%d)", strerror(errno), ret);
 282                 goto fail;
 283         }
 284
 285         if (user_format) {
 286                 format.fmt.pix.pixelformat = *fourcc;
 287                 format.fmt.pix.width = *width;
 288                 format.fmt.pix.height = *height;
 289         }
 290
 291         if ((format.fmt.pix.width == 0) || (format.fmt.pix.height == 0) ||
 292                         (format.fmt.pix.pixelformat == 0)) {
 293
 294                 ERROR("invalid capture settings '%dx%d@%4s' (did you not use '-c'?)",
 295                         format.fmt.pix.width, format.fmt.pix.height,
 296                         (char *)&format.fmt.pix.pixelformat);
 297                 goto fail;
 298         }
 299
 300         if (mcf) {
 301                 ret = media_setup(&format);
 302                 if (ret < 0) {
 303                         goto fail;
 304                 }
 305         }
 306
 307         ret = ioctl(v4l2->fd, VIDIOC_S_FMT, &format);
 308         if (ret < 0) {
 309                 ERROR("VIDIOC_S_FMT failed: %s (%d)", strerror(errno), ret);
 310                 goto fail;
 311         }
 312
 313         return v4l2;
 314
 315 fail:
 316         // XXX cleanup
 317         return NULL;
 318 }
 319
 320 int
 321 v4l2_reqbufs(struct v4l2 *v4l2, struct buffer **bufs, uint32_t n)
 322 {
 323         struct v4l2_requestbuffers reqbuf = {
 324                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
 325                         .memory = V4L2_MEMORY_DMABUF,
 326                         .count = n,
 327         };
 328         uint32_t i;
 329         int ret,dmafd;
 330
 331         if (v4l2->v4l2bufs) {
 332                 // maybe eventually need to support this?
 333                 ERROR("already reqbuf'd");
 334                 return -1;
 335         }
 336
 337         ret = ioctl(v4l2->fd, VIDIOC_REQBUFS, &reqbuf);
 338         if (ret < 0) {
 339                 ERROR("VIDIOC_REQBUFS failed: %s (%d)", strerror(errno), ret);
 340                 return ret;
 341         }
 342
 343         if ((reqbuf.count != n) ||
 344                         (reqbuf.type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
 345                         (reqbuf.memory != V4L2_MEMORY_DMABUF)) {
 346                 ERROR("unsupported..");
 347                 return -1;
 348         }
 349
 350         v4l2->nbufs = reqbuf.count;
 351         v4l2->bufs = bufs;
 352         v4l2->v4l2bufs = calloc(v4l2->nbufs, sizeof(*v4l2->v4l2bufs));
 353         if (!v4l2->v4l2bufs) {
 354                 ERROR("allocation failed");
 355                 return -1;
 356         }
 357
 358         for (i = 0; i < reqbuf.count; i++) {
 359                 assert(bufs[i]->nbo == 1); /* TODO add multi-planar support */
 360                 /* Call omap_bo_dmabuf only once, to export only once
 361                  * Otherwise, each call will return duplicated fds
 362                  * This way, every call to omap_bo_dmabuf will return a new fd
 363                  * Which won't match with any previously exported fds
 364                  * Instead, store dma fd in buf->fd[] */
 365                 dmafd = omap_bo_dmabuf(bufs[i]->bo[0]);
 366                 bufs[i]->fd[0] = dmafd;
 367                 v4l2->v4l2bufs[i] = (struct v4l2_buffer){
 368                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
 369                                         .memory = V4L2_MEMORY_DMABUF,
 370                                         .index = i,
 371                                         .m.fd = dmafd,
 372                 };
 373                 MSG("Exported buffer fd = %d\n", dmafd);
 374                 ret = ioctl(v4l2->fd, VIDIOC_QUERYBUF, &v4l2->v4l2bufs[i]);
 375                 v4l2->v4l2bufs[i].m.fd = dmafd;
 376                 if (ret) {
 377                         ERROR("VIDIOC_QUERYBUF failed: %s (%d)", strerror(errno), ret);
 378                         return ret;
 379                 }
 380         }
 381
 382         return 0;
 383 }
 384
 385 int
 386 v4l2_streamon(struct v4l2 *v4l2)
 387 {
 388         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 389         int ret;
 390
 391     ret = ioctl(v4l2->fd, VIDIOC_STREAMON, &type);
 392
 393     if (ret) {
 394                 ERROR("VIDIOC_STREAMON failed: %s (%d)", strerror(errno), ret);
 395     }
 396
 397     return ret;
 398 }
 399
 400 int
 401 v4l2_streamoff(struct v4l2 *v4l2)
 402 {
 403         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 404         int ret;
 405
 406     ret = ioctl(v4l2->fd, VIDIOC_STREAMOFF, &type);
 407
 408     if (ret) {
 409                 ERROR("VIDIOC_STREAMOFF failed: %s (%d)", strerror(errno), ret);
 410     }
 411
 412     return ret;
 413 }
 414
 415 int
 416 v4l2_qbuf(struct v4l2 *v4l2, struct buffer *buf)
 417 {
 418         struct v4l2_buffer *v4l2buf = NULL;
 419         int i, ret, fd;
 420
 421         assert(buf->nbo == 1); /* TODO add multi-planar support */
 422
 423         fd = buf->fd[0];
 424
 425         for (i = 0; i < v4l2->nbufs; i++) {
 426                 if (v4l2->v4l2bufs[i].m.fd == fd) {
 427                         v4l2buf = &v4l2->v4l2bufs[i];
 428                 }
 429         }
 430
 431         if (!v4l2buf) {
 432                 ERROR("invalid buffer");
 433                 return -1;
 434         }
 435
 436         MSG("QBUF: idx=%d, fd=%d", v4l2buf->index, v4l2buf->m.fd);
 437
 438         ret = ioctl(v4l2->fd, VIDIOC_QBUF, v4l2buf);
 439         v4l2buf->m.fd = buf->fd[0];
 440         if (ret) {
 441                 ERROR("VIDIOC_QBUF failed: %s (%d)", strerror(errno), ret);
 442         }
 443
 444         return ret;
 445 }
 446
 447 struct buffer *
 448 v4l2_dqbuf(struct v4l2 *v4l2)
 449 {
 450         struct buffer *buf;
 451         struct v4l2_buffer v4l2buf = {
 452                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
 453                         .memory = V4L2_MEMORY_DMABUF,
 454         };
 455         int ret;
 456
 457         ret = ioctl(v4l2->fd, VIDIOC_DQBUF, &v4l2buf);
 458         if (ret) {
 459                 ERROR("VIDIOC_DQBUF failed: %s (%d)", strerror(errno), ret);
 460         }
 461
 462         buf = v4l2->bufs[v4l2buf.index];
 463
 464         assert(buf->nbo == 1); /* TODO add multi-planar support */
 465
 466         MSG("DQBUF: idx=%d, fd=%d", v4l2buf.index, buf->fd[0]);
 467
 468         return buf;
 469 }