libs/grabbers/GP_V4L2.c

   1 /*****************************************************************************
   2  * This file is part of gfxprim library.                                     *
   3  *                                                                           *
   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.        *
   8  *                                                                           *
   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.                           *
  13  *                                                                           *
  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                                               *
  18  *                                                                           *
  19  * Copyright (C) 2009-2012 Cyril Hrubis <metan@ucw.cz>                       *
  20  *                                                                           *
  21  *****************************************************************************/
  22
  23  /*
  24
  25    Based on V4L2 example code.
  26
  27   */
  28
  29 #include <sys/types.h>
  30 #include <sys/stat.h>
  31 #include <fcntl.h>
  32 #include <sys/ioctl.h>
  33 #include <string.h>
  34 #include <errno.h>
  35 #include <sys/mman.h>
  36
  37 #include <stdint.h>
  38
  39 #include "core/GP_Context.h"
  40
  41 #include "core/GP_Debug.h"
  42
  43 #include "../../config.h"
  44
  45 #ifdef HAVE_V4L2
  46
  47 #include <linux/videodev2.h>
  48
  49 #include "GP_Grabber.h"
  50 #include "GP_V4L2.h"
  51
  52 struct v4l2_priv {
  53         int mode;
  54
  55         /* pointer to page aligned user buffer */
  56         void *bufptr[4];
  57         size_t buf_len[4];
  58
  59         char device[];
  60 };
  61
  62 static int v4l2_stop(struct GP_Grabber *self);
  63
  64 static void v4l2_exit(struct GP_Grabber *self)
  65 {
  66         struct v4l2_priv *priv = GP_GRABBER_PRIV(self);
  67         int i;
  68
  69         GP_DEBUG(1, "Grabber '%s' exitting", priv->device);
  70
  71         v4l2_stop(self);
  72
  73         if (priv->mode == 2) {
  74                 for (i = 0; i < 4; i++)
  75                         munmap(priv->bufptr[i], priv->buf_len[i]);
  76         }
  77
  78         close(self->fd);
  79         GP_ContextFree(self->frame);
  80         free(self);
  81 }
  82
  83 #define CLAMP(x, max)    \
  84         if (x > max)     \
  85                 x = max; \
  86         if (x < 0)       \
  87                 x = 0;   \
  88
  89 #define MUL 1024
  90
  91 static void v4l2_yuv422_fillframe(struct GP_Grabber *self, void *buf)
  92 {
  93         unsigned int i, j;
  94         unsigned char *py, *pu, *pv;
  95         unsigned char *tmp = self->frame->pixels;
  96
  97         py = buf;
  98         pu = buf + 1;
  99         pv = buf + 3;
 100
 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;
 105
 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;
 110
 111                         R = (R + MUL/2)/MUL;
 112                         G = (G + MUL/2)/MUL;
 113                         B = (B + MUL/2)/MUL;
 114
 115                         CLAMP(R, 255);
 116                         CLAMP(G, 255);
 117                         CLAMP(B, 255);
 118
 119                         *tmp++ = R;
 120                         *tmp++ = G;
 121                         *tmp++ = G;
 122
 123                         py += 2;
 124
 125                         if ((j & 1) == 1) {
 126                                 pu += 4;
 127                                 pv += 4;
 128                         }
 129                 }
 130         }
 131 }
 132
 133 static int v4l2_poll(struct GP_Grabber *self)
 134 {
 135         struct v4l2_priv *priv = GP_GRABBER_PRIV(self);
 136
 137         GP_DEBUG(3, "Grabber '%s' poll", priv->device);
 138
 139         /* read/write interface */
 140         if (priv->mode == 1) {
 141                 GP_WARN("Read/write I/O not implemented.");
 142                 return 0;
 143         }
 144
 145         /* mmaped interface */
 146         struct v4l2_buffer buf;
 147
 148         memset(&buf, 0, sizeof(buf));
 149
 150         buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 151         buf.memory = V4L2_MEMORY_MMAP;
 152
 153         if (ioctl(self->fd, VIDIOC_DQBUF, &buf)) {
 154                 switch (errno) {
 155                 case EAGAIN:
 156                         return 0;
 157                 default:
 158                         GP_WARN("Failed to ioctl VIDIOC_DQBUF on '%s' : %s",
 159                                 priv->device, strerror(errno));
 160                         return 0;
 161                 }
 162         }
 163
 164         if (buf.index >= 4) {
 165                 GP_WARN("Got invalid buffer index on '%s'", priv->device);
 166                 return 0;
 167         }
 168
 169         v4l2_yuv422_fillframe(self, priv->bufptr[buf.index]);
 170
 171         if (ioctl(self->fd, VIDIOC_QBUF, &buf)) {
 172                 GP_WARN("Failed to ioctl VIDIOC_QBUF on '%s' : %s",
 173                         priv->device, strerror(errno));
 174         }
 175
 176         return 1;
 177 }
 178
 179 static int v4l2_start(struct GP_Grabber *self)
 180 {
 181         struct v4l2_priv *priv = GP_GRABBER_PRIV(self);
 182
 183         /* read/write interface */
 184         if (priv->mode == 1)
 185                 return 0;
 186
 187         /* mmap interface */
 188         int i;
 189         struct v4l2_buffer buf;
 190         memset(&buf, 0, sizeof(buf));
 191
 192         for (i = 0; i < 4; i++) {
 193                 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 194                 buf.memory = V4L2_MEMORY_MMAP;
 195                 buf.index = i;
 196
 197                 if (ioctl(self->fd, VIDIOC_QBUF, &buf)) {
 198                         GP_WARN("Failed to ioclt VIDIOC_QBUF on '%s': %s",
 199                                 priv->device, strerror(errno));
 200                         return 1;
 201                 }
 202         }
 203
 204         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 205
 206         if (ioctl(self->fd, VIDIOC_STREAMON, &type)) {
 207                 GP_WARN("Failed to ioclt VIDIOC_STREAMON on '%s': %s",
 208                         priv->device, strerror(errno));
 209                 return 1;
 210         }
 211
 212         return 0;
 213 }
 214
 215 static int v4l2_stop(struct GP_Grabber *self)
 216 {
 217         struct v4l2_priv *priv = GP_GRABBER_PRIV(self);
 218
 219         /* read/write interface */
 220         if (priv->mode == 1)
 221                 return 0;
 222
 223         /* mmap interface */
 224         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 225
 226         if (ioctl(self->fd, VIDIOC_STREAMOFF, &type)) {
 227                 GP_WARN("Failed to ioclt VIDIOC_STREAMON on '%s': %s",
 228                         priv->device, strerror(errno));
 229                 return 1;
 230         }
 231
 232         return 0;
 233 }
 234
 235 struct GP_Grabber *GP_GrabberV4L2Init(const char *device,
 236                                       unsigned int prefered_width,
 237                                       unsigned int prefered_height)
 238 {
 239         int fd, i, err;
 240         int mode = 0;
 241
 242         GP_DEBUG(1, "Opening V4L2 grabber '%s'", device);
 243
 244         fd = open(device, O_RDWR | O_NONBLOCK);
 245
 246         if (fd < 0) {
 247                 err = errno;
 248                 GP_WARN("Failed to open V4L2 grabber '%s'", device);
 249                 goto err;
 250         }
 251
 252         struct v4l2_capability cap;
 253
 254         if (ioctl(fd, VIDIOC_QUERYCAP, &cap)) {
 255                 err = errno;
 256                 GP_WARN("ioctl VIDIOC_QUERYCAP failed, '%s' not V4L2 device?",
 257                         device);
 258                 goto err0;
 259         }
 260
 261         if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
 262                 err = ENOSYS;
 263                 GP_WARN("Device '%s' has no capture capability", device);
 264                 goto err0;
 265         }
 266
 267
 268         if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
 269                 GP_DEBUG(1, "Device '%s' doesn't support read write I/O", device);
 270         } else {
 271                 mode = 1;
 272         }
 273
 274         if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
 275                 GP_DEBUG(1, "Device '%s' doesn't support streaming I/O", device);
 276         } else {
 277                 mode = 2;
 278         }
 279
 280         if (mode == 0) {
 281                 err = ENOSYS;
 282                 GP_WARN("No suitable mode found for '%s'", device);
 283                 goto err0;
 284         }
 285
 286         struct v4l2_cropcap cropcap;
 287         struct v4l2_crop crop;
 288
 289         memset(&cropcap, 0, sizeof(cropcap));
 290
 291         cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 292
 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;
 297
 298                 if (ioctl(fd, VIDIOC_S_CROP, &crop)) {
 299                         /* error/cropping not supported */
 300                 }
 301
 302         } else {
 303                 /* errors ignored? */
 304         }
 305
 306         struct v4l2_format fmt;
 307
 308         memset(&fmt, 0, sizeof(fmt));
 309
 310         fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 311         fmt.fmt.pix.width       = prefered_width;
 312         fmt.fmt.pix.height      = prefered_height;
 313         fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
 314         fmt.fmt.pix.field       = V4L2_FIELD_INTERLACED;
 315
 316         if (ioctl(fd, VIDIOC_S_FMT, &fmt)) {
 317                 err = errno;
 318                 GP_WARN("Failed to set video format for device '%s'", device);
 319                 goto err0;
 320         }
 321
 322         GP_Grabber *new = malloc(sizeof(GP_Grabber) + sizeof(struct v4l2_priv) + strlen(device) + 1);
 323
 324         if (new == NULL) {
 325                 err = ENOMEM;
 326                 GP_WARN("Malloc failed :(");
 327                 goto err0;
 328         }
 329
 330         new->frame = GP_ContextAlloc(fmt.fmt.pix.width, fmt.fmt.pix.height, GP_PIXEL_RGB888);
 331
 332         if (new->frame == NULL) {
 333                 err = ENOMEM;
 334                 goto err1;
 335         }
 336
 337         struct v4l2_priv *priv = GP_GRABBER_PRIV(new);
 338
 339         strcpy(priv->device, device);
 340         priv->mode = mode;
 341
 342         switch (mode) {
 343         case 1:
 344         break;
 345         case 2: {
 346                 /* setup mmap interface */
 347                 struct v4l2_requestbuffers req;
 348
 349                 memset(&req, 0, sizeof(req));
 350
 351                 req.count  = 4;
 352                 req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 353                 req.memory = V4L2_MEMORY_MMAP;
 354
 355                 if (ioctl(fd, VIDIOC_REQBUFS, &req)) {
 356                         err = errno;
 357                         GP_WARN("Failed to ioctl VIDIOC_REQBUFS on '%s' : %s",
 358                                 device, strerror(errno));
 359                         goto err2;
 360                 }
 361
 362                 if (req.count != 4) {
 363                         err = ENOSYS;
 364                         GP_WARN("Unexpected number of buffers on '%s'", device);
 365                         goto err2;
 366                 }
 367
 368                 struct v4l2_buffer buf;
 369                 memset(&buf, 0, sizeof(buf));
 370
 371                 priv->bufptr[0] = NULL;
 372                 priv->bufptr[1] = NULL;
 373                 priv->bufptr[2] = NULL;
 374                 priv->bufptr[3] = NULL;
 375
 376                 for (i = 0; i < 4; i++) {
 377                         buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 378                         buf.memory = V4L2_MEMORY_MMAP;
 379                         buf.index = i;
 380
 381                         if (ioctl(fd, VIDIOC_QUERYBUF, &buf)) {
 382                                 err = errno;
 383                                 GP_WARN("Failed to ioclt VIDIOC_QUERYBUF on '%s': %s",
 384                                         device, strerror(errno));
 385                                 goto err2;
 386                         }
 387
 388                         priv->bufptr[i] = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
 389                         priv->buf_len[i] = buf.length;
 390
 391                         if (priv->bufptr[i] == MAP_FAILED) {
 392                                 err = errno;
 393                                 GP_WARN("mmap failed on '%s': %s", device, strerror(errno));
 394                                 goto err3;
 395                         }
 396                 }
 397         } break;
 398         }
 399
 400         new->fd = fd;
 401         new->Exit = v4l2_exit;
 402         new->Poll = v4l2_poll;
 403         new->Start = v4l2_start;
 404         new->Stop = v4l2_stop;
 405
 406         return new;
 407 err3:
 408         for (i = 0; i < 4; i++)
 409                 if (priv->bufptr[i] != NULL)
 410                         munmap(priv->bufptr[i], priv->buf_len[i]);
 411 err2:
 412         GP_ContextFree(new->frame);
 413 err1:
 414         free(new);
 415 err0:
 416         close(fd);
 417 err:
 418         errno = err;
 419         return NULL;
 420 }
 421
 422 #else
 423
 424 struct GP_Grabber *GP_GrabberV4L2Init(const char GP_UNUSED(*device),
 425                                       unsigned int GP_UNUSED(prefered_width),
 426                                       unsigned int GP_UNUSED(prefered_height))
 427 {
 428         GP_WARN("V4L2 support not compiled in.");
 429
 430         errno = ENOSYS;
 431         return NULL;
 432 }
 433
 434 #endif /* HAVE_V4L2 */