add video buffer lock/unlock API
[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 #include <linux/videodev2.h>
19 #include <sys/types.h>
20 #include <sys/stat.h>
21 #include <fcntl.h>
22 #include <sys/ioctl.h>
24 #include "util.h"
26 struct v4l2 {
27         int fd;
28         int nbufs;
29         struct v4l2_buffer *v4l2bufs;
30         struct buffer **bufs;
31 };
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 }
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>
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;
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;
65         target[ret] = '\0';
66         p = strrchr(target, '/');
67         if (p == NULL)
68                 return -EINVAL;
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;
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         }
85         return -1;
86 }
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;
94         for (id = 0; ; id = entity->id) {
95                 memset(entity, 0, sizeof(*entity));
96                 entity->id = id | MEDIA_ENT_ID_FLAG_NEXT;
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                 }
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);
109                 if ((entity->type == type) || (entity->id == entity_id)) {
110                         return 0;
111                 }
112         }
114         return ret;
117 static void
118 media_configure(struct media_entity_desc *entity,
119                 struct v4l2_format *format, int pad)
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;
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         }
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         }
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)
156         struct media_entity_desc entity;
157         int fd, ret;
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         }
165         ret = media_find_entity(fd, &entity, MEDIA_ENT_T_V4L2_SUBDEV_SENSOR, ~0);
166         if (ret) {
167                 return ret;
168         }
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;
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                 }
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..
191                                 media_configure(&entity, format, links[i].source.index);
192                                 media_configure(&entity, format, links[i].sink.index);
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                                 }
205                                 ret = media_find_entity(fd, &entity, ~0, links[i].sink.entity);
206                                 if (ret) {
207                                         return ret;
208                                 }
210                                 break;
211                         }
212                 }
213         } while (entity.type != MEDIA_ENT_T_DEVNODE_V4L);
215         return 0;
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)
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;
230         v4l2 = calloc(1, sizeof(*v4l2));
231         v4l2->fd = open("/dev/video0", O_RDWR);
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         }
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         }
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         }
274         *fourcc = format.fmt.pix.pixelformat;
275         *width  = format.fmt.pix.width;
276         *height = format.fmt.pix.height;
278         if (mcf) {
279                 ret = media_setup(&format);
280                 if (ret < 0) {
281                         goto fail;
282                 }
283         }
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         }
291         return v4l2;
293 fail:
294         // XXX cleanup
295         return NULL;
298 int
299 v4l2_reqbufs(struct v4l2 *v4l2, struct buffer **bufs, uint32_t n)
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;
309         if (v4l2->v4l2bufs) {
310                 // maybe eventually need to support this?
311                 ERROR("already reqbuf'd");
312                 return -1;
313         }
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         }
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         }
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         }
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         }
351         return 0;
354 int
355 v4l2_streamon(struct v4l2 *v4l2)
357         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
358         int ret;
360     ret = ioctl(v4l2->fd, VIDIOC_STREAMON, &type);
362     if (ret) {
363                 ERROR("VIDIOC_STREAMON failed: %s (%d)", strerror(errno), ret);
364     }
366     return ret;
369 int
370 v4l2_streamoff(struct v4l2 *v4l2)
372         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
373         int ret;
375     ret = ioctl(v4l2->fd, VIDIOC_STREAMOFF, &type);
377     if (ret) {
378                 ERROR("VIDIOC_STREAMOFF failed: %s (%d)", strerror(errno), ret);
379     }
381     return ret;
384 int
385 v4l2_qbuf(struct v4l2 *v4l2, struct buffer *buf)
387         struct v4l2_buffer *v4l2buf = NULL;
388         int i, ret, fd;
390         assert(buf->nbo == 1); /* TODO add multi-planar support */
392         fd = omap_bo_dmabuf(buf->bo[0]);
394         for (i = 0; i < v4l2->nbufs; i++) {
395                 if (v4l2->v4l2bufs[i].m.fd == fd) {
396                         v4l2buf = &v4l2->v4l2bufs[i];
397                 }
398         }
400         if (!v4l2buf) {
401                 ERROR("invalid buffer");
402                 return -1;
403         }
405         MSG("QBUF: idx=%d, fd=%d", v4l2buf->index, v4l2buf->m.fd);
407         ret = ioctl(v4l2->fd, VIDIOC_QBUF, v4l2buf);
408         if (ret) {
409                 ERROR("VIDIOC_QBUF failed: %s (%d)", strerror(errno), ret);
410         }
412         return ret;
415 struct buffer *
416 v4l2_dqbuf(struct v4l2 *v4l2)
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;
425         ret = ioctl(v4l2->fd, VIDIOC_DQBUF, &v4l2buf);
426         if (ret) {
427                 ERROR("VIDIOC_DQBUF failed: %s (%d)", strerror(errno), ret);
428         }
430         MSG("DQBUF: idx=%d, fd=%d", v4l2buf.index, v4l2buf.m.fd);
432         buf = v4l2->bufs[v4l2buf.index];
434         assert(buf->nbo == 1); /* TODO add multi-planar support */
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         }
441         return buf;