1 /*****************************************************************************
2 * This file is part of gfxprim library. *
4 * Gfxprim is free software; you can redistribute it and/or *
5 * modify it under the terms of the GNU Lesser General Public *
6 * License as published by the Free Software Foundation; either *
7 * version 2.1 of the License, or (at your option) any later version. *
9 * Gfxprim is distributed in the hope that it will be useful, *
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
12 * Lesser General Public License for more details. *
14 * You should have received a copy of the GNU Lesser General Public *
15 * License along with gfxprim; if not, write to the Free Software *
16 * Foundation, Inc., 51 Franklin Street, Fifth Floor, *
17 * Boston, MA 02110-1301 USA *
19 * Copyright (C) 2009-2012 Cyril Hrubis <metan@ucw.cz> *
21 *****************************************************************************/
25 Based on V4L2 example code.
29 #include <sys/types.h>
32 #include <sys/ioctl.h>
39 #include "core/GP_Pixmap.h"
41 #include "core/GP_Debug.h"
43 #include "../../config.h"
47 #include <linux/videodev2.h>
49 #include "GP_Grabber.h"
55 /* pointer to page aligned user buffer */
62 static int v4l2_stop(struct GP_Grabber
*self
);
64 static void v4l2_exit(struct GP_Grabber
*self
)
66 struct v4l2_priv
*priv
= GP_GRABBER_PRIV(self
);
69 GP_DEBUG(1, "Grabber '%s' exitting", priv
->device
);
73 if (priv
->mode
== 2) {
74 for (i
= 0; i
< 4; i
++)
75 munmap(priv
->bufptr
[i
], priv
->buf_len
[i
]);
79 GP_PixmapFree(self
->frame
);
83 #define CLAMP(x, max) \
91 static void v4l2_yuv422_fillframe(struct GP_Grabber
*self
, void *buf
)
94 unsigned char *py
, *pu
, *pv
;
95 unsigned char *tmp
= self
->frame
->pixels
;
101 for (i
= 0; i
< self
->frame
->h
; i
++) {
102 for (j
= 0; j
< self
->frame
->w
; j
++) {
103 int32_t PU
= ((int32_t)*pu
) - 128;
104 int32_t PV
= ((int32_t)*pv
) - 128;
106 int32_t R
= MUL
* (*py
) + ((int32_t)(MUL
* 1.772)) * PU
;
107 int32_t G
= MUL
* (*py
) - ((int32_t)(MUL
* 0.344)) * PU
108 - ((int32_t)(MUL
* 0.714)) * PV
;
109 int32_t B
= MUL
* (*py
) + ((int32_t)(MUL
* 1.402)) * PV
;
133 static int v4l2_poll(struct GP_Grabber
*self
)
135 struct v4l2_priv
*priv
= GP_GRABBER_PRIV(self
);
137 GP_DEBUG(3, "Grabber '%s' poll", priv
->device
);
139 /* read/write interface */
140 if (priv
->mode
== 1) {
141 GP_WARN("Read/write I/O not implemented.");
145 /* mmaped interface */
146 struct v4l2_buffer buf
;
148 memset(&buf
, 0, sizeof(buf
));
150 buf
.type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
151 buf
.memory
= V4L2_MEMORY_MMAP
;
153 if (ioctl(self
->fd
, VIDIOC_DQBUF
, &buf
)) {
158 GP_WARN("Failed to ioctl VIDIOC_DQBUF on '%s' : %s",
159 priv
->device
, strerror(errno
));
164 if (buf
.index
>= 4) {
165 GP_WARN("Got invalid buffer index on '%s'", priv
->device
);
169 v4l2_yuv422_fillframe(self
, priv
->bufptr
[buf
.index
]);
171 if (ioctl(self
->fd
, VIDIOC_QBUF
, &buf
)) {
172 GP_WARN("Failed to ioctl VIDIOC_QBUF on '%s' : %s",
173 priv
->device
, strerror(errno
));
179 static int v4l2_start(struct GP_Grabber
*self
)
181 struct v4l2_priv
*priv
= GP_GRABBER_PRIV(self
);
183 /* read/write interface */
189 struct v4l2_buffer buf
;
190 memset(&buf
, 0, sizeof(buf
));
192 for (i
= 0; i
< 4; i
++) {
193 buf
.type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
194 buf
.memory
= V4L2_MEMORY_MMAP
;
197 if (ioctl(self
->fd
, VIDIOC_QBUF
, &buf
)) {
198 GP_WARN("Failed to ioclt VIDIOC_QBUF on '%s': %s",
199 priv
->device
, strerror(errno
));
204 enum v4l2_buf_type type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
206 if (ioctl(self
->fd
, VIDIOC_STREAMON
, &type
)) {
207 GP_WARN("Failed to ioclt VIDIOC_STREAMON on '%s': %s",
208 priv
->device
, strerror(errno
));
215 static int v4l2_stop(struct GP_Grabber
*self
)
217 struct v4l2_priv
*priv
= GP_GRABBER_PRIV(self
);
219 /* read/write interface */
224 enum v4l2_buf_type type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
226 if (ioctl(self
->fd
, VIDIOC_STREAMOFF
, &type
)) {
227 GP_WARN("Failed to ioclt VIDIOC_STREAMON on '%s': %s",
228 priv
->device
, strerror(errno
));
235 struct GP_Grabber
*GP_GrabberV4L2Init(const char *device
,
236 unsigned int preferred_width
,
237 unsigned int preferred_height
)
242 GP_DEBUG(1, "Opening V4L2 grabber '%s'", device
);
244 fd
= open(device
, O_RDWR
| O_NONBLOCK
);
248 GP_WARN("Failed to open V4L2 grabber '%s'", device
);
252 struct v4l2_capability cap
;
254 if (ioctl(fd
, VIDIOC_QUERYCAP
, &cap
)) {
256 GP_WARN("ioctl VIDIOC_QUERYCAP failed, '%s' not V4L2 device?",
261 if (!(cap
.capabilities
& V4L2_CAP_VIDEO_CAPTURE
)) {
263 GP_WARN("Device '%s' has no capture capability", device
);
268 if (!(cap
.capabilities
& V4L2_CAP_READWRITE
)) {
269 GP_DEBUG(1, "Device '%s' doesn't support read write I/O", device
);
274 if (!(cap
.capabilities
& V4L2_CAP_STREAMING
)) {
275 GP_DEBUG(1, "Device '%s' doesn't support streaming I/O", device
);
282 GP_WARN("No suitable mode found for '%s'", device
);
286 struct v4l2_cropcap cropcap
;
287 struct v4l2_crop crop
;
289 memset(&cropcap
, 0, sizeof(cropcap
));
291 cropcap
.type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
293 if (ioctl(fd
, VIDIOC_CROPCAP
, &cropcap
) == 0) {
294 crop
.type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
295 /* reset to default */
296 crop
.c
= cropcap
.defrect
;
298 if (ioctl(fd
, VIDIOC_S_CROP
, &crop
)) {
299 /* error/cropping not supported */
302 GP_WARN("ioctl VIDIOC_CROPCAP failed: %s",
308 struct v4l2_format fmt
;
310 memset(&fmt
, 0, sizeof(fmt
));
312 fmt
.type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
313 fmt
.fmt
.pix
.width
= preferred_width
;
314 fmt
.fmt
.pix
.height
= preferred_height
;
315 fmt
.fmt
.pix
.pixelformat
= V4L2_PIX_FMT_YUYV
;
316 fmt
.fmt
.pix
.field
= V4L2_FIELD_INTERLACED
;
318 if (ioctl(fd
, VIDIOC_S_FMT
, &fmt
)) {
320 GP_WARN("Failed to set video format for device '%s'", device
);
324 GP_Grabber
*new = malloc(sizeof(GP_Grabber
) + sizeof(struct v4l2_priv
) + strlen(device
) + 1);
328 GP_WARN("Malloc failed :(");
332 new->frame
= GP_PixmapAlloc(fmt
.fmt
.pix
.width
, fmt
.fmt
.pix
.height
, GP_PIXEL_RGB888
);
334 if (new->frame
== NULL
) {
339 struct v4l2_priv
*priv
= GP_GRABBER_PRIV(new);
341 strcpy(priv
->device
, device
);
348 /* setup mmap interface */
349 struct v4l2_requestbuffers req
;
351 memset(&req
, 0, sizeof(req
));
354 req
.type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
355 req
.memory
= V4L2_MEMORY_MMAP
;
357 if (ioctl(fd
, VIDIOC_REQBUFS
, &req
)) {
359 GP_WARN("Failed to ioctl VIDIOC_REQBUFS on '%s' : %s",
360 device
, strerror(errno
));
364 if (req
.count
!= 4) {
366 GP_WARN("Unexpected number of buffers on '%s'", device
);
370 struct v4l2_buffer buf
;
371 memset(&buf
, 0, sizeof(buf
));
373 priv
->bufptr
[0] = NULL
;
374 priv
->bufptr
[1] = NULL
;
375 priv
->bufptr
[2] = NULL
;
376 priv
->bufptr
[3] = NULL
;
378 for (i
= 0; i
< 4; i
++) {
379 buf
.type
= V4L2_BUF_TYPE_VIDEO_CAPTURE
;
380 buf
.memory
= V4L2_MEMORY_MMAP
;
383 if (ioctl(fd
, VIDIOC_QUERYBUF
, &buf
)) {
385 GP_WARN("Failed to ioclt VIDIOC_QUERYBUF on '%s': %s",
386 device
, strerror(errno
));
390 priv
->bufptr
[i
] = mmap(NULL
, buf
.length
, PROT_READ
| PROT_WRITE
, MAP_SHARED
, fd
, buf
.m
.offset
);
391 priv
->buf_len
[i
] = buf
.length
;
393 if (priv
->bufptr
[i
] == MAP_FAILED
) {
395 GP_WARN("mmap failed on '%s': %s", device
, strerror(errno
));
403 new->Exit
= v4l2_exit
;
404 new->Poll
= v4l2_poll
;
405 new->Start
= v4l2_start
;
406 new->Stop
= v4l2_stop
;
410 for (i
= 0; i
< 4; i
++)
411 if (priv
->bufptr
[i
] != NULL
)
412 munmap(priv
->bufptr
[i
], priv
->buf_len
[i
]);
414 GP_PixmapFree(new->frame
);
426 struct GP_Grabber
*GP_GrabberV4L2Init(const char GP_UNUSED(*device
),
427 unsigned int GP_UNUSED(preferred_width
),
428 unsigned int GP_UNUSED(preferred_height
))
430 GP_WARN("V4L2 support not compiled in.");
436 #endif /* HAVE_V4L2 */