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>
29 #include "util.h"
31 struct v4l2 {
32 int fd;
33 int nbufs;
34 struct v4l2_buffer *v4l2bufs;
35 struct buffer **bufs;
36 };
38 void
39 v4l2_usage(void)
40 {
41 MSG("V4L2 Capture Options:");
42 MSG("\t-c WxH@fourcc\tset capture dimensions/format");
43 MSG("\t-m\t\tdo MCF setup");
44 }
46 /* media_* helpers to do the MCF dance to get things configured properly
47 * so that we can set the specified format on the output device. For
48 * non-MCF cameras this can just be skipped.
49 */
50 #include <linux/media.h>
51 #include <linux/v4l2-subdev.h>
53 static int
54 media_open_entity(struct media_entity_desc *entity)
55 {
56 struct stat devstat;
57 char devname[32];
58 char sysname[32];
59 char target[1024];
60 char *p;
61 int ret;
63 sprintf(sysname, "/sys/dev/char/%u:%u", entity->v4l.major,
64 entity->v4l.minor);
65 ret = readlink(sysname, target, sizeof(target));
66 if (ret < 0)
67 return -errno;
70 target[ret] = '\0';
71 p = strrchr(target, '/');
72 if (p == NULL)
73 return -EINVAL;
75 sprintf(devname, "/dev/%s", p + 1);
76 MSG("\t%s -> %s -> %s", sysname, target, devname);
77 ret = stat(devname, &devstat);
78 if (ret < 0)
79 return -errno;
81 /* Sanity check: udev might have reordered the device nodes.
82 * Make sure the major/minor match. We should really use
83 * libudev.
84 */
85 if (major(devstat.st_rdev) == entity->v4l.major &&
86 minor(devstat.st_rdev) == entity->v4l.minor) {
87 return open(devname, O_RDWR);
88 }
90 return -1;
91 }
93 static int
94 media_find_entity(int fd, struct media_entity_desc *entity,
95 uint32_t type, uint32_t entity_id)
96 {
97 int id, ret = 0;
99 for (id = 0; ; id = entity->id) {
100 memset(entity, 0, sizeof(*entity));
101 entity->id = id | MEDIA_ENT_ID_FLAG_NEXT;
103 ret = ioctl(fd, MEDIA_IOC_ENUM_ENTITIES, entity);
104 if (ret) {
105 ERROR("MEDIA_IOC_ENUM_ENTITIES failed: %s (%d)",
106 strerror(errno), ret);
107 break;
108 }
110 MSG("\tfound entity: %s type=%08x, flags=%08x, group_id=%d, pads=%d, links=%d",
111 entity->name, entity->type, entity->flags, entity->group_id,
112 entity->pads, entity->links);
114 if ((entity->type == type) || (entity->id == entity_id)) {
115 return 0;
116 }
117 }
119 return ret;
120 }
122 static void
123 media_configure(struct media_entity_desc *entity,
124 struct v4l2_format *format, int pad)
125 {
126 struct v4l2_subdev_format ent_format = {
127 .pad = pad,
128 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
129 .format = {
130 .width = format->fmt.pix.width,
131 .height = format->fmt.pix.height,
132 .code = V4L2_MBUS_FMT_UYVY8_1X16,
133 .field = V4L2_FIELD_NONE,
134 .colorspace = V4L2_COLORSPACE_JPEG,
135 },
136 };
137 int fd, ret;
139 fd = media_open_entity(entity);
140 if (fd < 0) {
141 ERROR("could not open media device: \"%s\" (%d:%d)", entity->name,
142 entity->v4l.major, entity->v4l.minor);
143 return;
144 }
146 MSG("Setting format for: \"%s\" (%d)", entity->name, pad);
147 ret = ioctl(fd, VIDIOC_SUBDEV_S_FMT, &ent_format);
148 if (ret) {
149 MSG("Could not configure: %s (%d)", strerror(errno), ret);
150 }
151 }
153 /* walk the graph and attempt to configure all the nodes to the same settings.
154 * This works for smart-sensor with no element in between that can convert/
155 * scale.. If the sensor can't handle the settings, then S_FMT just fails
156 * and hopefully some element in between can pick up the slack.
157 */
158 static int
159 media_setup(struct v4l2_format *format)
160 {
161 struct media_entity_desc entity;
162 int fd, ret;
164 fd = open("/dev/media0", O_RDWR);
165 if (fd < 0) {
166 ERROR("could not open MCF: %s (%d)", strerror(errno), ret);
167 return fd;
168 }
170 ret = media_find_entity(fd, &entity, MEDIA_ENT_T_V4L2_SUBDEV_SENSOR, ~0);
171 if (ret) {
172 return ret;
173 }
175 /* now walk the graph to the output, configure everything on the way: */
176 do {
177 struct media_link_desc links[10];
178 struct media_links_enum link_enum = {
179 .entity = entity.id,
180 .links = links,
181 };
182 int i;
184 ret = ioctl(fd, MEDIA_IOC_ENUM_LINKS, &link_enum);
185 if (ret) {
186 ERROR("MEDIA_IOC_ENUM_LINKS failed: %s (%d)",
187 strerror(errno), ret);
188 return ret;
189 }
191 for (i = 0; i < entity.links; i++) {
192 if (links[i].source.entity == entity.id) {
193 // XXX maybe if there are multiple links, we should prefer
194 // an enabled link, otherwise just pick one..
196 media_configure(&entity, format, links[i].source.index);
197 media_configure(&entity, format, links[i].sink.index);
199 /* lets take this link.. */
200 if (!(links[i].flags & MEDIA_LNK_FL_ENABLED)) {
201 links[i].flags |= MEDIA_LNK_FL_ENABLED;
202 ret = ioctl(fd, MEDIA_IOC_SETUP_LINK, &links[i]);
203 if (ret) {
204 ERROR("MEDIA_IOC_SETUP_LINK failed: %s (%d)",
205 strerror(errno), errno);
206 // return ret;
207 }
208 }
210 ret = media_find_entity(fd, &entity, ~0, links[i].sink.entity);
211 if (ret) {
212 return ret;
213 }
215 break;
216 }
217 }
218 } while (entity.type != MEDIA_ENT_T_DEVNODE_V4L);
220 return 0;
221 }
223 /* Open v4l2 (and media0??) XXX */
224 struct v4l2 *
225 v4l2_open(int argc, char **argv, uint32_t *fourcc,
226 uint32_t *width, uint32_t *height)
227 {
228 struct v4l2_format format = {
229 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
230 };
231 struct v4l2 *v4l2;
232 int i, ret;
233 bool mcf = false, user_format = false, user_dev = false;
234 char devname[20] = "/dev/video1";
236 v4l2 = calloc(1, sizeof(*v4l2));
238 /* note: set args to NULL after we've parsed them so other modules know
239 * that it is already parsed (since the arg parsing is decentralized)
240 */
241 for (i = 1; i < argc; i++) {
242 if (!argv[i]) {
243 continue;
244 }
245 if (!strcmp("-d", argv[i])) {
246 if(user_dev)
247 continue;
249 argv[i++] = NULL;
250 if (sscanf(argv[i], "%s", devname) != 1) {
251 ERROR("invalid device node: %s", argv[i]);
252 goto fail;
253 }
254 user_dev = true;
255 } else if (!strcmp("-c", argv[i])) {
256 if(user_format)
257 continue;
259 char fourccstr[5];
260 argv[i++] = NULL;
261 if (sscanf(argv[i], "%ux%u@%4s",
262 width,
263 height,
264 fourccstr) != 3) {
265 ERROR("invalid arg: %s", argv[i]);
266 goto fail;
267 }
268 *fourcc = FOURCC_STR(fourccstr);
269 user_format = true;
270 } else if (!strcmp(argv[i], "-m")) {
271 mcf = true;
272 } else {
273 continue;
274 }
275 argv[i] = NULL;
276 }
278 v4l2->fd = open(devname, O_RDWR);
280 ret = ioctl(v4l2->fd, VIDIOC_G_FMT, &format);
281 if (ret < 0) {
282 ERROR("VIDIOC_G_FMT failed: %s (%d)", strerror(errno), ret);
283 goto fail;
284 }
286 if (user_format) {
287 format.fmt.pix.pixelformat = *fourcc;
288 format.fmt.pix.width = *width;
289 format.fmt.pix.height = *height;
290 if(*fourcc == FOURCC_STR("AR24"))
291 format.fmt.pix.pixelformat = FOURCC_STR("RGB4");
293 }
295 if ((format.fmt.pix.width == 0) || (format.fmt.pix.height == 0) ||
296 (format.fmt.pix.pixelformat == 0)) {
298 ERROR("invalid capture settings '%dx%d@%4s' (did you not use '-c'?)",
299 format.fmt.pix.width, format.fmt.pix.height,
300 (char *)&format.fmt.pix.pixelformat);
301 goto fail;
302 }
304 if (mcf) {
305 ret = media_setup(&format);
306 if (ret < 0) {
307 goto fail;
308 }
309 }
311 ret = ioctl(v4l2->fd, VIDIOC_S_FMT, &format);
312 if (ret < 0) {
313 ERROR("VIDIOC_S_FMT failed: %s (%d)", strerror(errno), ret);
314 goto fail;
315 }
317 return v4l2;
319 fail:
320 // XXX cleanup
321 return NULL;
322 }
324 int
325 v4l2_reqbufs(struct v4l2 *v4l2, struct buffer **bufs, uint32_t n)
326 {
327 struct v4l2_requestbuffers reqbuf = {
328 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
329 .memory = V4L2_MEMORY_DMABUF,
330 .count = n,
331 };
332 uint32_t i;
333 int ret,dmafd;
335 if (v4l2->v4l2bufs) {
336 // maybe eventually need to support this?
337 ERROR("already reqbuf'd");
338 return -1;
339 }
341 ret = ioctl(v4l2->fd, VIDIOC_REQBUFS, &reqbuf);
342 if (ret < 0) {
343 ERROR("VIDIOC_REQBUFS failed: %s (%d)", strerror(errno), ret);
344 return ret;
345 }
347 if ((reqbuf.count != n) ||
348 (reqbuf.type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
349 (reqbuf.memory != V4L2_MEMORY_DMABUF)) {
350 ERROR("unsupported..");
351 return -1;
352 }
354 v4l2->nbufs = reqbuf.count;
355 v4l2->bufs = bufs;
356 v4l2->v4l2bufs = calloc(v4l2->nbufs, sizeof(*v4l2->v4l2bufs));
357 if (!v4l2->v4l2bufs) {
358 ERROR("allocation failed");
359 return -1;
360 }
362 for (i = 0; i < reqbuf.count; i++) {
363 assert(bufs[i]->nbo == 1); /* TODO add multi-planar support */
364 /* Call omap_bo_dmabuf only once, to export only once
365 * Otherwise, each call will return duplicated fds
366 * This way, every call to omap_bo_dmabuf will return a new fd
367 * Which won't match with any previously exported fds
368 * Instead, store dma fd in buf->fd[] */
369 dmafd = omap_bo_dmabuf(bufs[i]->bo[0]);
370 bufs[i]->fd[0] = dmafd;
371 v4l2->v4l2bufs[i] = (struct v4l2_buffer){
372 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
373 .memory = V4L2_MEMORY_DMABUF,
374 .index = i,
375 .m.fd = dmafd,
376 };
377 MSG("Exported buffer fd = %d\n", dmafd);
378 ret = ioctl(v4l2->fd, VIDIOC_QUERYBUF, &v4l2->v4l2bufs[i]);
379 v4l2->v4l2bufs[i].m.fd = dmafd;
380 if (ret) {
381 ERROR("VIDIOC_QUERYBUF failed: %s (%d)", strerror(errno), ret);
382 return ret;
383 }
384 }
386 return 0;
387 }
389 int
390 v4l2_streamon(struct v4l2 *v4l2)
391 {
392 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
393 int ret;
395 ret = ioctl(v4l2->fd, VIDIOC_STREAMON, &type);
397 if (ret) {
398 ERROR("VIDIOC_STREAMON failed: %s (%d)", strerror(errno), ret);
399 }
401 return ret;
402 }
404 int
405 v4l2_streamoff(struct v4l2 *v4l2)
406 {
407 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
408 int ret;
410 ret = ioctl(v4l2->fd, VIDIOC_STREAMOFF, &type);
412 if (ret) {
413 ERROR("VIDIOC_STREAMOFF failed: %s (%d)", strerror(errno), ret);
414 }
416 return ret;
417 }
419 int
420 v4l2_qbuf(struct v4l2 *v4l2, struct buffer *buf)
421 {
422 struct v4l2_buffer *v4l2buf = NULL;
423 int i, ret, fd;
425 struct timeval disp_time;
426 struct timeval disp_lat;
427 pthread_t tid;
429 assert(buf->nbo == 1); /* TODO add multi-planar support */
431 fd = buf->fd[0];
433 for (i = 0; i < v4l2->nbufs; i++) {
434 if (v4l2->v4l2bufs[i].m.fd == fd) {
435 v4l2buf = &v4l2->v4l2bufs[i];
436 }
437 }
439 tid = pthread_self();
440 gettimeofday(&disp_time, NULL);
441 timersub(&disp_time, &v4l2buf->timestamp, &disp_lat);
442 if(v4l2buf->timestamp.tv_sec)
443 DBG("%x: DISP latency = %d.%6d", tid, disp_lat.tv_sec, disp_lat.tv_usec);
445 if (!v4l2buf) {
446 ERROR("invalid buffer");
447 return -1;
448 }
450 ret = ioctl(v4l2->fd, VIDIOC_QBUF, v4l2buf);
451 v4l2buf->m.fd = buf->fd[0];
452 if (ret) {
453 ERROR("VIDIOC_QBUF failed: %s (%d)", strerror(errno), ret);
454 }
456 return ret;
457 }
459 struct buffer *
460 v4l2_dqbuf(struct v4l2 *v4l2)
461 {
462 struct timeval dq_time;
463 struct timeval dq_lat;
464 pthread_t tid;
466 struct buffer *buf;
467 struct v4l2_buffer v4l2buf = {
468 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
469 .memory = V4L2_MEMORY_DMABUF,
470 };
471 int ret;
473 ret = ioctl(v4l2->fd, VIDIOC_DQBUF, &v4l2buf);
474 if (ret) {
475 ERROR("VIDIOC_DQBUF failed: %s (%d)", strerror(errno), ret);
476 }
478 buf = v4l2->bufs[v4l2buf.index];
479 v4l2->v4l2bufs[v4l2buf.index].timestamp = v4l2buf.timestamp;
481 assert(buf->nbo == 1); /* TODO add multi-planar support */
483 tid = pthread_self();
484 gettimeofday(&dq_time, NULL);
485 timersub(&dq_time, &v4l2buf.timestamp, &dq_lat);
486 DBG("%x: DQ latency = %d.%6d", tid, dq_lat.tv_sec, dq_lat.tv_usec);
488 return buf;
489 }