omx cam: add test app
[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>
28 #include "util.h"
30 struct v4l2 {
31         int fd;
32         int nbufs;
33         struct v4l2_buffer *v4l2bufs;
34         struct buffer **bufs;
35 };
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 }
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>
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;
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;
69         target[ret] = '\0';
70         p = strrchr(target, '/');
71         if (p == NULL)
72                 return -EINVAL;
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;
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         }
89         return -1;
90 }
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;
98         for (id = 0; ; id = entity->id) {
99                 memset(entity, 0, sizeof(*entity));
100                 entity->id = id | MEDIA_ENT_ID_FLAG_NEXT;
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                 }
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);
113                 if ((entity->type == type) || (entity->id == entity_id)) {
114                         return 0;
115                 }
116         }
118         return ret;
121 static void
122 media_configure(struct media_entity_desc *entity,
123                 struct v4l2_format *format, int pad)
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;
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         }
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         }
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)
160         struct media_entity_desc entity;
161         int fd, ret;
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         }
169         ret = media_find_entity(fd, &entity, MEDIA_ENT_T_V4L2_SUBDEV_SENSOR, ~0);
170         if (ret) {
171                 return ret;
172         }
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;
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                 }
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..
195                                 media_configure(&entity, format, links[i].source.index);
196                                 media_configure(&entity, format, links[i].sink.index);
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                                 }
209                                 ret = media_find_entity(fd, &entity, ~0, links[i].sink.entity);
210                                 if (ret) {
211                                         return ret;
212                                 }
214                                 break;
215                         }
216                 }
217         } while (entity.type != MEDIA_ENT_T_DEVNODE_V4L);
219         return 0;
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)
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;
234         v4l2 = calloc(1, sizeof(*v4l2));
235         v4l2->fd = open("/dev/video0", O_RDWR);
237         ret = ioctl(v4l2->fd, VIDIOC_G_FMT, &format);
238         if (ret < 0) {
239                 ERROR("VIDIOC_G_FMT failed: %s (%d)", strerror(errno), ret);
240                 goto fail;
241         }
243         /* note: set args to NULL after we've parsed them so other modules know
244          * that it is already parsed (since the arg parsing is decentralized)
245          */
246         for (i = 1; i < argc; i++) {
247                 if (!argv[i]) {
248                         continue;
249                 }
250                 if (!strcmp("-c", argv[i])) {
251                         char fourccstr[5];
252                         argv[i++] = NULL;
253                         if (sscanf(argv[i], "%ux%u@%4s",
254                                         &format.fmt.pix.width,
255                                         &format.fmt.pix.height,
256                                         fourccstr) != 3) {
257                                 ERROR("invalid arg: %s", argv[i]);
258                                 goto fail;
259                         }
260                         format.fmt.pix.pixelformat = FOURCC_STR(fourccstr);
261                 } else if (!strcmp(argv[i], "-m")) {
262                         mcf = true;
263                 } else {
264                         continue;
265                 }
266                 argv[i] = NULL;
267         }
269         if ((format.fmt.pix.width == 0) ||
270                         (format.fmt.pix.height == 0) ||
271                         (format.fmt.pix.pixelformat == 0)) {
272                 ERROR("invalid capture settings '%dx%d@%4s' (did you not use '-c'?)",
273                                 format.fmt.pix.width, format.fmt.pix.height,
274                                 (char *)&format.fmt.pix.pixelformat);
275                 goto fail;
276         }
278         *fourcc = format.fmt.pix.pixelformat;
279         *width  = format.fmt.pix.width;
280         *height = format.fmt.pix.height;
282         if (mcf) {
283                 ret = media_setup(&format);
284                 if (ret < 0) {
285                         goto fail;
286                 }
287         }
289         ret = ioctl(v4l2->fd, VIDIOC_S_FMT, &format);
290         if (ret < 0) {
291                 ERROR("VIDIOC_S_FMT failed: %s (%d)", strerror(errno), ret);
292                 goto fail;
293         }
295         return v4l2;
297 fail:
298         // XXX cleanup
299         return NULL;
302 int
303 v4l2_reqbufs(struct v4l2 *v4l2, struct buffer **bufs, uint32_t n)
305         struct v4l2_requestbuffers reqbuf = {
306                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
307                         .memory = V4L2_MEMORY_DMABUF,
308                         .count = n,
309         };
310         uint32_t i;
311         int ret;
313         if (v4l2->v4l2bufs) {
314                 // maybe eventually need to support this?
315                 ERROR("already reqbuf'd");
316                 return -1;
317         }
319         ret = ioctl(v4l2->fd, VIDIOC_REQBUFS, &reqbuf);
320         if (ret < 0) {
321                 ERROR("VIDIOC_REQBUFS failed: %s (%d)", strerror(errno), ret);
322                 return ret;
323         }
325         if ((reqbuf.count != n) ||
326                         (reqbuf.type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
327                         (reqbuf.memory != V4L2_MEMORY_DMABUF)) {
328                 ERROR("unsupported..");
329                 return -1;
330         }
332         v4l2->nbufs = reqbuf.count;
333         v4l2->bufs = bufs;
334         v4l2->v4l2bufs = calloc(v4l2->nbufs, sizeof(*v4l2->v4l2bufs));
335         if (!v4l2->v4l2bufs) {
336                 ERROR("allocation failed");
337                 return -1;
338         }
340         for (i = 0; i < reqbuf.count; i++) {
341                 assert(bufs[i]->nbo == 1); /* TODO add multi-planar support */
342                 v4l2->v4l2bufs[i] = (struct v4l2_buffer){
343                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
344                                         .memory = V4L2_MEMORY_DMABUF,
345                                         .index = i,
346                                         .m.fd = omap_bo_dmabuf(bufs[i]->bo[0]),
347                 };
348                 ret = ioctl(v4l2->fd, VIDIOC_QUERYBUF, &v4l2->v4l2bufs[i]);
349                 v4l2->v4l2bufs[i].m.fd = omap_bo_dmabuf(bufs[i]->bo[0]);
350                 if (ret) {
351                         ERROR("VIDIOC_QUERYBUF failed: %s (%d)", strerror(errno), ret);
352                         return ret;
353                 }
354         }
356         return 0;
359 int
360 v4l2_streamon(struct v4l2 *v4l2)
362         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
363         int ret;
365     ret = ioctl(v4l2->fd, VIDIOC_STREAMON, &type);
367     if (ret) {
368                 ERROR("VIDIOC_STREAMON failed: %s (%d)", strerror(errno), ret);
369     }
371     return ret;
374 int
375 v4l2_streamoff(struct v4l2 *v4l2)
377         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
378         int ret;
380     ret = ioctl(v4l2->fd, VIDIOC_STREAMOFF, &type);
382     if (ret) {
383                 ERROR("VIDIOC_STREAMOFF failed: %s (%d)", strerror(errno), ret);
384     }
386     return ret;
389 int
390 v4l2_qbuf(struct v4l2 *v4l2, struct buffer *buf)
392         struct v4l2_buffer *v4l2buf = NULL;
393         int i, ret, fd;
395         assert(buf->nbo == 1); /* TODO add multi-planar support */
397         fd = omap_bo_dmabuf(buf->bo[0]);
399         for (i = 0; i < v4l2->nbufs; i++) {
400                 if (v4l2->v4l2bufs[i].m.fd == fd) {
401                         v4l2buf = &v4l2->v4l2bufs[i];
402                 }
403         }
405         if (!v4l2buf) {
406                 ERROR("invalid buffer");
407                 return -1;
408         }
410         MSG("QBUF: idx=%d, fd=%d", v4l2buf->index, v4l2buf->m.fd);
412         ret = ioctl(v4l2->fd, VIDIOC_QBUF, v4l2buf);
413         v4l2buf->m.fd = omap_bo_dmabuf(buf->bo[0]);
414         if (ret) {
415                 ERROR("VIDIOC_QBUF failed: %s (%d)", strerror(errno), ret);
416         }
418         return ret;
421 struct buffer *
422 v4l2_dqbuf(struct v4l2 *v4l2)
424         struct buffer *buf;
425         struct v4l2_buffer v4l2buf = {
426                         .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
427                         .memory = V4L2_MEMORY_DMABUF,
428         };
429         int ret;
431         ret = ioctl(v4l2->fd, VIDIOC_DQBUF, &v4l2buf);
432         if (ret) {
433                 ERROR("VIDIOC_DQBUF failed: %s (%d)", strerror(errno), ret);
434         }
436         buf = v4l2->bufs[v4l2buf.index];
438         assert(buf->nbo == 1); /* TODO add multi-planar support */
440         MSG("DQBUF: idx=%d, fd=%d", v4l2buf.index, omap_bo_dmabuf(buf->bo[0]));
442         return buf;