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