capture.cpp

   1 /*  video capture for rawv
   2  *  Copyright (C) 2010  Kirill Smelkov <kirr@navytux.spb.ru>
   3  *  Copyright (C) 2011  Marine Bridge and Navigation Systems (http://mns.spb.ru/)
   4  *
   5  *  This library is free software: you can Use, Study, Modify and Redistribute
   6  *  it under the terms of the GNU Lesser General Public License version 2.1, or
   7  *  any later version. This library is distributed WITHOUT ANY WARRANTY. See
   8  *  COPYING.LIB file for full License terms.
   9  *
  10  *  V4L2 stuff, based on
  11  *  http://v4l2spec.bytesex.org/spec/capture-example.html
  12  */
  13 #include "rawv.h"
  14
  15 #include <linux/videodev2.h>
  16 #include <sys/stat.h>
  17 #include <sys/types.h>
  18 #include <sys/ioctl.h>
  19 #include <sys/mman.h>
  20 #include <unistd.h>
  21 #include <fcntl.h>              /* low-level i/o */
  22 #include <errno.h>
  23 #include <string.h>
  24 #include <assert.h>
  25
  26 #include <stdio.h>
  27
  28 #define CLEAR(x) memset (&(x), 0, sizeof (x))
  29
  30 namespace rawv {
  31
  32 /******** VideoCapture ********/
  33
  34 VideoCapture::VideoCapture(const char *dev_name, int width, int height, int interlace_tb_swapped, int queue_len)
  35 {
  36     struct v4l2_capability cap;
  37     struct v4l2_format fmt;
  38     struct v4l2_requestbuffers req;
  39     unsigned i;
  40     char *p;
  41
  42     vcapture_on = 0;
  43
  44     fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
  45     if (-1 == fd)
  46         die ("Cannot open '%s': %d, %s", dev_name, errno, strerror (errno));
  47
  48     if (-1 == ioctl (fd, VIDIOC_QUERYCAP, &cap))
  49         die_errno ("VIDIOC_QUERYCAP");
  50
  51     if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
  52         die ("%s is no video capture device", dev_name);
  53
  54     if (!(cap.capabilities & V4L2_CAP_STREAMING))
  55         die ("%s does not support streaming i/o", dev_name);
  56
  57
  58     /* Select video input, video standard and tune here. */
  59     CLEAR (fmt);
  60
  61     fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  62     fmt.fmt.pix.width       = width;
  63     fmt.fmt.pix.height      = height;
  64     fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
  65     fmt.fmt.pix.field       = V4L2_FIELD_NONE /*i.e. progressive*/;
  66
  67     if (-1 == ioctl (fd, VIDIOC_S_FMT, &fmt)) {
  68             warn_errno ("VIDIOC_S_FMT(..., progressive) failed -- will try interlaced...");
  69
  70             /* FIXME - better query dev capabilities in the first place */
  71             fmt.fmt.pix.field   = V4L2_FIELD_INTERLACED;
  72             if (-1 == ioctl (fd, VIDIOC_S_FMT, &fmt))
  73                 die_errno ("VIDIOC_S_FMT(..., interlaced)");
  74     }
  75
  76
  77
  78     /* Note VIDIOC_S_FMT may change width and height. */
  79     width  = fmt.fmt.pix.width;
  80     height = fmt.fmt.pix.height;
  81
  82     p = (char *)&fmt.fmt.pix.pixelformat;
  83     fprintf(stderr, "Capturing  '%c%c%c%c'  h: %i  w: %i  stride: %i  bt: %i\n",
  84             p[0], p[1], p[2], p[3],
  85             fmt.fmt.pix.height, fmt.fmt.pix.width, fmt.fmt.pix.bytesperline,
  86             interlace_tb_swapped);
  87
  88
  89     /* setup mmap capture */
  90     CLEAR (req);
  91
  92     req.count               = queue_len;
  93     req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  94     req.memory              = V4L2_MEMORY_MMAP;
  95
  96     if (-1 == ioctl (fd, VIDIOC_REQBUFS, &req))
  97         die_errno("VIDIOC_REQBUFS");
  98
  99     if (req.count < 2)
 100         die("Insufficient buffer memory on %s", dev_name);
 101
 102
 103     buffers.resize(req.count);
 104
 105     for (i = 0; i < req.count; ++i) {
 106         struct v4l2_buffer buf;
 107
 108         CLEAR (buf);
 109
 110         buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 111         buf.memory      = V4L2_MEMORY_MMAP;
 112         buf.index       = i;
 113
 114         if (-1 == ioctl (fd, VIDIOC_QUERYBUF, &buf))
 115             die_errno ("VIDIOC_QUERYBUF");
 116
 117         buffers[i].width  = fmt.fmt.pix.width;
 118         buffers[i].height = fmt.fmt.pix.height;
 119         buffers[i].bytesperline = fmt.fmt.pix.bytesperline;
 120         buffers[i].pixfmt_4cc = fmt.fmt.pix.pixelformat;    // XXX ok?
 121         buffers[i].sequence = -1UL;
 122
 123         buffers[i].interlace_tb_swapped = interlace_tb_swapped;
 124
 125         buffers[i].length = buf.length;
 126         buffers[i].start  = (uint8_t *)
 127                 mmap (NULL /* start anywhere */,
 128                       buf.length,
 129                       PROT_READ | PROT_WRITE /* required */,
 130                       MAP_SHARED /* recommended */,
 131                       fd, buf.m.offset);
 132
 133         if (MAP_FAILED == buffers[i].start)
 134             die_errno ("mmap");
 135     }
 136 }
 137
 138
 139
 140 VideoCapture::~VideoCapture()
 141 {
 142     unsigned int i;
 143
 144     /* munmap */
 145     for (i = 0; i < buffers.size(); ++i)
 146         if (-1 == munmap (buffers[i].start, buffers[i].length))
 147             die_errno ("munmap");
 148
 149
 150     if (-1 == close (fd))
 151         die_errno ("close");
 152
 153     fd = -1;
 154 }
 155
 156
 157
 158 void VideoCapture::start_capture()
 159 {
 160     unsigned int i;
 161     enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 162
 163     if (vcapture_on)
 164         return; /* already started */
 165
 166     for (i = 0; i < buffers.size(); ++i) {
 167         struct v4l2_buffer buf;
 168
 169         CLEAR (buf);
 170
 171         buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 172         buf.memory      = V4L2_MEMORY_MMAP;
 173         buf.index       = i;
 174
 175         if (-1 == ioctl (fd, VIDIOC_QBUF, &buf))
 176             die_errno ("VIDIOC_QBUF (start_capture)");
 177     }
 178
 179     if (-1 == ioctl (fd, VIDIOC_STREAMON, &type))
 180         die_errno ("VIDIOC_STREAMON");
 181
 182     vcapture_on = 1;
 183 }
 184
 185
 186 void VideoCapture::stop_capture()
 187 {
 188     enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 189
 190     if (!vcapture_on)
 191         return; /* already stopped */
 192
 193     if (-1 == ioctl (fd, VIDIOC_STREAMOFF, &type))
 194         die_errno ("VIDIOC_STREAMOFF");
 195
 196     vcapture_on = 0;
 197 }
 198
 199
 200
 201 void VideoCapture::subscribe(void (*func)(const Frame *, void *), void *self)
 202 {
 203     FrameSubscription fs;
 204     fs.func = func;
 205     fs.self = self;
 206
 207     subscribers.push_back(fs);
 208 }
 209
 210 void VideoCapture::unsubscribe(void (*func)(const Frame *, void *), void *self)
 211 {
 212     FrameSubscription fs;
 213     fs.func = func;
 214     fs.self = self;
 215
 216     for (vector<FrameSubscription>::iterator it=subscribers.begin(); it != subscribers.end();) {
 217         if (fs == *it)
 218             subscribers.erase(it);
 219         else
 220             ++it;
 221     }
 222 }
 223
 224
 225
 226
 227
 228 int VideoCapture::read_frame(void)
 229 {
 230     unsigned i;
 231     struct v4l2_buffer buf;
 232
 233     CLEAR (buf);
 234
 235     buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 236     buf.memory = V4L2_MEMORY_MMAP;
 237
 238     if (-1 == ioctl (fd, VIDIOC_DQBUF, &buf))
 239         switch (errno) {
 240             case EAGAIN:
 241                 return 0;
 242
 243             case EIO:
 244                 /* Could ignore EIO, see spec. */
 245
 246                 /* fall through */
 247
 248             default:
 249                 die_errno ("VIDIOC_DQBUF");
 250         }
 251
 252     assert (buf.index < buffers.size());
 253
 254     buffers[buf.index].sequence = buf.sequence;
 255
 256     /* go through subscribers & notify them */
 257     for (i=0; i<subscribers.size(); ++i) {
 258         FrameSubscription fs = subscribers[i];
 259
 260         fs.func(&buffers[buf.index], fs.self);
 261     }
 262
 263     if (-1 == ioctl (fd, VIDIOC_QBUF, &buf))
 264         die_errno ("VIDIOC_QBUF (read_frame)");
 265
 266     return 1;
 267 }
 268
 269 }   // rawv::