5e5e5bf7db03133c4b87aeb62748d77300b90bb4
[glsdk/omapdrmtest.git] / 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  */
18 #ifdef HAVE_CONFIG_H
19 #include "config.h"
20 #endif
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 #include <pthread.h>
28 #include <time.h>
30 #include "util.h"
32 struct v4l2 {
33         int fd;
34         int nbufs;
35         struct v4l2_buffer *v4l2bufs;
36         struct buffer **bufs;
37 };
39 void
40 v4l2_usage(void)
41 {
42         MSG("V4L2 Capture Options:");
43         MSG("\t-c WxH@fourcc\tset capture dimensions/format");
44         MSG("\t-m\t\tdo MCF setup");
45 }
47 /* media_* helpers to do the MCF dance to get things configured properly
48  * so that we can set the specified format on the output device.  For
49  * non-MCF cameras this can just be skipped.
50  */
51 #include <linux/media.h>
52 #include <linux/v4l2-subdev.h>
54 static int
55 media_open_entity(struct media_entity_desc *entity)
56 {
57         struct stat devstat;
58         char devname[32];
59         char sysname[32];
60         char target[1024];
61         char *p;
62         int ret;
64         sprintf(sysname, "/sys/dev/char/%u:%u", entity->v4l.major,
65                         entity->v4l.minor);
66         ret = readlink(sysname, target, sizeof(target));
67         if (ret < 0)
68                 return -errno;
71         target[ret] = '\0';
72         p = strrchr(target, '/');
73         if (p == NULL)
74                 return -EINVAL;
76         sprintf(devname, "/dev/%s", p + 1);
77 MSG("\t%s -> %s -> %s", sysname, target, devname);
78         ret = stat(devname, &devstat);
79         if (ret < 0)
80                 return -errno;
82         /* Sanity check: udev might have reordered the device nodes.
83          * Make sure the major/minor match. We should really use
84          * libudev.
85          */
86         if (major(devstat.st_rdev) == entity->v4l.major &&
87             minor(devstat.st_rdev) == entity->v4l.minor) {
88                 return open(devname, O_RDWR);
89         }
91         return -1;
92 }
94 static int
95 media_find_entity(int fd, struct media_entity_desc *entity,
96                 uint32_t type, uint32_t entity_id)
97 {
98         int id, ret = 0;
100         for (id = 0; ; id = entity->id) {
101                 memset(entity, 0, sizeof(*entity));
102                 entity->id = id | MEDIA_ENT_ID_FLAG_NEXT;
104                 ret = ioctl(fd, MEDIA_IOC_ENUM_ENTITIES, entity);
105                 if (ret) {
106                         ERROR("MEDIA_IOC_ENUM_ENTITIES failed: %s (%d)",
107                                         strerror(errno), ret);
108                         break;
109                 }
111 MSG("\tfound entity: %s type=%08x, flags=%08x, group_id=%d, pads=%d, links=%d",
112 entity->name, entity->type, entity->flags, entity->group_id,
113 entity->pads, entity->links);
115                 if ((entity->type == type) || (entity->id == entity_id)) {
116                         return 0;
117                 }
118         }
120         return ret;
123 static void
124 media_configure(struct media_entity_desc *entity,
125                 struct v4l2_format *format, int pad)
127         struct v4l2_subdev_format ent_format = {
128                         .pad = pad,
129                         .which = V4L2_SUBDEV_FORMAT_ACTIVE,
130                         .format = {
131                                         .width  = format->fmt.pix.width,
132                                         .height = format->fmt.pix.height,
133                                         .code   = V4L2_MBUS_FMT_UYVY8_1X16,
134                                         .field  = V4L2_FIELD_NONE,
135                                         .colorspace = V4L2_COLORSPACE_JPEG,
136                         },
137         };
138         int fd, ret;
140         fd = media_open_entity(entity);
141         if (fd < 0) {
142                 ERROR("could not open media device: \"%s\" (%d:%d)", entity->name,
143                                 entity->v4l.major, entity->v4l.minor);
144                 return;
145         }
147         MSG("Setting format for: \"%s\" (%d)", entity->name, pad);
148         ret = ioctl(fd, VIDIOC_SUBDEV_S_FMT, &ent_format);
149         if (ret) {
150                 MSG("Could not configure: %s (%d)", strerror(errno), ret);
151         }
154 /* walk the graph and attempt to configure all the nodes to the same settings.
155  * This works for smart-sensor with no element in between that can convert/
156  * scale..  If the sensor can't handle the settings, then S_FMT just fails
157  * and hopefully some element in between can pick up the slack.
158  */
159 static int
160 media_setup(struct v4l2_format *format)
162         struct media_entity_desc entity;
163         int fd, ret;
165         fd = open("/dev/media0", O_RDWR);
166         if (fd < 0) {
167                 ERROR("could not open MCF: %s (%d)", strerror(errno), ret);
168                 return fd;
169         }
171         ret = media_find_entity(fd, &entity, MEDIA_ENT_T_V4L2_SUBDEV_SENSOR, ~0);
172         if (ret) {
173                 return ret;
174         }
176         /* now walk the graph to the output, configure everything on the way: */
177         do {
178                 struct media_link_desc links[10];
179                 struct media_links_enum link_enum = {
180                                 .entity = entity.id,
181                                 .links = links,
182                 };
183                 int i;
185                 ret = ioctl(fd, MEDIA_IOC_ENUM_LINKS, &link_enum);
186                 if (ret) {
187                         ERROR("MEDIA_IOC_ENUM_LINKS failed: %s (%d)",
188                                         strerror(errno), ret);
189                         return ret;
190                 }
192                 for (i = 0; i < entity.links; i++) {
193                         if (links[i].source.entity == entity.id) {
194                                 // XXX maybe if there are multiple links, we should prefer
195                                 // an enabled link, otherwise just pick one..
197                                 media_configure(&entity, format, links[i].source.index);
198                                 media_configure(&entity, format, links[i].sink.index);
200                                 /* lets take this link.. */
201                                 if (!(links[i].flags & MEDIA_LNK_FL_ENABLED)) {
202                                         links[i].flags |= MEDIA_LNK_FL_ENABLED;
203                                         ret = ioctl(fd, MEDIA_IOC_SETUP_LINK, &links[i]);
204                                         if (ret) {
205                                                 ERROR("MEDIA_IOC_SETUP_LINK failed: %s (%d)",
206                                                                 strerror(errno), errno);
207 //                                              return ret;
208                                         }
209                                 }
211                                 ret = media_find_entity(fd, &entity, ~0, links[i].sink.entity);
212                                 if (ret) {
213                                         return ret;
214                                 }
216                                 break;
217                         }
218                 }
219         } while (entity.type != MEDIA_ENT_T_DEVNODE_V4L);
221         return 0;
224 /* Open v4l2 (and media0??) XXX */
225 struct v4l2 *
226 v4l2_open(int argc, char **argv, uint32_t *fourcc,
227                 uint32_t *width, uint32_t *height)
229         struct v4l2_format format = {
230                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
231         };
232         struct v4l2 *v4l2;
233         int i, ret;
234         bool mcf = false, user_format = false, user_dev = false;
235         char devname[20] = "/dev/video1";
237         v4l2 = calloc(1, sizeof(*v4l2));
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("-d", argv[i])) {
247                         if(user_dev)
248                                 continue;
250                         argv[i++] = NULL;
251                         if (sscanf(argv[i], "%s", devname) != 1) {
252                                 ERROR("invalid device node: %s", argv[i]);
253                                 goto fail;
254                         }
255                         user_dev = true;
256                 } else if (!strcmp("-c", argv[i])) {
257                         if(user_format)
258                                 continue;
260                         char fourccstr[5];
261                         argv[i++] = NULL;
262                         if (sscanf(argv[i], "%ux%u@%4s",
263                                         width,
264                                         height,
265                                         fourccstr) != 3) {
266                                 ERROR("invalid arg: %s", argv[i]);
267                                 goto fail;
268                         }
269                         *fourcc = FOURCC_STR(fourccstr);
270                         user_format = true;
271                 } else if (!strcmp(argv[i], "-m")) {
272                         mcf = true;
273                 } else {
274                         continue;
275                 }
276                 argv[i] = NULL;
277         }
279         v4l2->fd = open(devname, O_RDWR);
281         ret = ioctl(v4l2->fd, VIDIOC_G_FMT, &format);
282         if (ret < 0) {
283                 ERROR("VIDIOC_G_FMT failed: %s (%d)", strerror(errno), ret);
284                 goto fail;
285         }
287         if (user_format) {
288                 format.fmt.pix.pixelformat = *fourcc;
289                 format.fmt.pix.width = *width;
290                 format.fmt.pix.height = *height;
291                 if(*fourcc == FOURCC_STR("AR24"))
292                         format.fmt.pix.pixelformat = FOURCC_STR("RGB4");
294         }
296         if ((format.fmt.pix.width == 0) || (format.fmt.pix.height == 0) ||
297                         (format.fmt.pix.pixelformat == 0)) {
299                 ERROR("invalid capture settings '%dx%d@%4s' (did you not use '-c'?)",
300                         format.fmt.pix.width, format.fmt.pix.height,
301                         (char *)&format.fmt.pix.pixelformat);
302                 goto fail;
303         }
305         if (mcf) {
306                 ret = media_setup(&format);
307                 if (ret < 0) {
308                         goto fail;
309                 }
310         }
312         ret = ioctl(v4l2->fd, VIDIOC_S_FMT, &format);
313         if (ret < 0) {
314                 ERROR("VIDIOC_S_FMT failed: %s (%d)", strerror(errno), ret);
315                 goto fail;
316         }
318         return v4l2;
320 fail:
321         // XXX cleanup
322         return NULL;
325 int
326 v4l2_reqbufs(struct v4l2 *v4l2, struct buffer **bufs, uint32_t n)
328         struct v4l2_requestbuffers reqbuf = {
329                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
330                         .memory = V4L2_MEMORY_DMABUF,
331                         .count = n,
332         };
333         uint32_t i;
334         int ret,dmafd;
336         if (v4l2->v4l2bufs) {
337                 // maybe eventually need to support this?
338                 ERROR("already reqbuf'd");
339                 return -1;
340         }
342         ret = ioctl(v4l2->fd, VIDIOC_REQBUFS, &reqbuf);
343         if (ret < 0) {
344                 ERROR("VIDIOC_REQBUFS failed: %s (%d)", strerror(errno), ret);
345                 return ret;
346         }
348         if ((reqbuf.count != n) ||
349                         (reqbuf.type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
350                         (reqbuf.memory != V4L2_MEMORY_DMABUF)) {
351                 ERROR("unsupported..");
352                 return -1;
353         }
355         v4l2->nbufs = reqbuf.count;
356         v4l2->bufs = bufs;
357         v4l2->v4l2bufs = calloc(v4l2->nbufs, sizeof(*v4l2->v4l2bufs));
358         if (!v4l2->v4l2bufs) {
359                 ERROR("allocation failed");
360                 return -1;
361         }
363         for (i = 0; i < reqbuf.count; i++) {
364                 assert(bufs[i]->nbo == 1); /* TODO add multi-planar support */
365                 /* Call omap_bo_dmabuf only once, to export only once
366                  * Otherwise, each call will return duplicated fds
367                  * This way, every call to omap_bo_dmabuf will return a new fd
368                  * Which won't match with any previously exported fds
369                  * Instead, store dma fd in buf->fd[] */
370                 dmafd = omap_bo_dmabuf(bufs[i]->bo[0]);
371                 bufs[i]->fd[0] = dmafd;
372                 v4l2->v4l2bufs[i] = (struct v4l2_buffer){
373                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
374                                         .memory = V4L2_MEMORY_DMABUF,
375                                         .index = i,
376                                         .m.fd = dmafd,
377                 };
378                 MSG("Exported buffer fd = %d\n", dmafd);
379                 ret = ioctl(v4l2->fd, VIDIOC_QUERYBUF, &v4l2->v4l2bufs[i]);
380                 v4l2->v4l2bufs[i].m.fd = dmafd;
381                 if (ret) {
382                         ERROR("VIDIOC_QUERYBUF failed: %s (%d)", strerror(errno), ret);
383                         return ret;
384                 }
385         }
387         return 0;
390 int
391 v4l2_streamon(struct v4l2 *v4l2)
393         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
394         int ret;
396     ret = ioctl(v4l2->fd, VIDIOC_STREAMON, &type);
398     if (ret) {
399                 ERROR("VIDIOC_STREAMON failed: %s (%d)", strerror(errno), ret);
400     }
402     return ret;
405 int
406 v4l2_streamoff(struct v4l2 *v4l2)
408         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
409         int ret;
411     ret = ioctl(v4l2->fd, VIDIOC_STREAMOFF, &type);
413     if (ret) {
414                 ERROR("VIDIOC_STREAMOFF failed: %s (%d)", strerror(errno), ret);
415     }
417     return ret;
420 int
421 v4l2_qbuf(struct v4l2 *v4l2, struct buffer *buf)
423         struct v4l2_buffer *v4l2buf = NULL;
424         int i, ret, fd;
426         struct timeval disp_time;
427         struct timeval disp_lat;
428         struct timespec now;
429         pthread_t tid;
431         assert(buf->nbo == 1); /* TODO add multi-planar support */
433         fd = buf->fd[0];
435         for (i = 0; i < v4l2->nbufs; i++) {
436                 if (v4l2->v4l2bufs[i].m.fd == fd) {
437                         v4l2buf = &v4l2->v4l2bufs[i];
438                 }
439         }
441         tid = pthread_self();
442         clock_gettime(CLOCK_MONOTONIC, &now);
443         disp_time.tv_sec = now.tv_sec;
444         disp_time.tv_usec = now.tv_nsec / 1000;
445         timersub(&disp_time, &v4l2buf->timestamp, &disp_lat);
446         if(v4l2buf->timestamp.tv_sec)
447                 DBG("%x: DISP latency = %d.%6d", tid, disp_lat.tv_sec, disp_lat.tv_usec);
449         if (!v4l2buf) {
450                 ERROR("invalid buffer");
451                 return -1;
452         }
454         ret = ioctl(v4l2->fd, VIDIOC_QBUF, v4l2buf);
455         v4l2buf->m.fd = buf->fd[0];
456         if (ret) {
457                 ERROR("VIDIOC_QBUF failed: %s (%d)", strerror(errno), ret);
458         }
460         return ret;
463 struct buffer *
464 v4l2_dqbuf(struct v4l2 *v4l2)
466         struct timeval dq_time;
467         struct timeval dq_lat;
468         struct timespec now;
469         pthread_t tid;
471         struct buffer *buf;
472         struct v4l2_buffer v4l2buf = {
473                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
474                         .memory = V4L2_MEMORY_DMABUF,
475         };
476         int ret;
478         ret = ioctl(v4l2->fd, VIDIOC_DQBUF, &v4l2buf);
479         if (ret) {
480                 ERROR("VIDIOC_DQBUF failed: %s (%d)", strerror(errno), ret);
481         }
483         buf = v4l2->bufs[v4l2buf.index];
484         v4l2->v4l2bufs[v4l2buf.index].timestamp = v4l2buf.timestamp;
486         assert(buf->nbo == 1); /* TODO add multi-planar support */
488         tid = pthread_self();
489         clock_gettime(CLOCK_MONOTONIC, &now);
490         dq_time.tv_sec = now.tv_sec;
491         dq_time.tv_usec = now.tv_nsec / 1000;
492         timersub(&dq_time, &v4l2buf.timestamp, &dq_lat);
493         DBG("%x: DQ latency = %d.%6d", tid, dq_lat.tv_sec, dq_lat.tv_usec);
495         return buf;