Работа с usb видеокамерой в Linux. Часть 2

int videodevice::xioctl(int fd, int request, void *arg)
{
 int r;

 r = ioctl (fd, request, arg);

 if(r == -1)
 {
 if (errno == EAGAIN)
 return EAGAIN;

 stringstream ss;

 ss << "ioctl code " << request << " ";

 errno_exit(ss.str());
 }

 return r;
}

void videodevice::getFrame(string file_name)
{
 initMMAP();

 startCapturing();

 long int i = 0;

 for (;;)
 {
 if(readFrame(file_name))
 break;

 i++;
 }

 cout << "iter == " << i << endl;

 stopCapturing();

 freeMMAP();
}

void videodevice::initMMAP()
{
 struct v4l2_requestbuffers req;

 req.count = 1;
 req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 req.memory = V4L2_MEMORY_MMAP;

 xioctl(fd, VIDIOC_REQBUFS, &req);

 devbuffer = (buffer*) calloc(req.count, sizeof(*devbuffer));

 struct v4l2_buffer buf;

 memset(&buf, 0, sizeof(buf));

 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 buf.memory = V4L2_MEMORY_MMAP;
 buf.index = 0;

 xioctl(fd, VIDIOC_QUERYBUF, &buf);

 devbuffer->length = buf.length;
 devbuffer->start =
 mmap(NULL,
 buf.length,
 PROT_READ | PROT_WRITE,
 MAP_SHARED,
 fd,
 buf.m.offset);

 if (devbuffer->start == MAP_FAILED)
 errno_exit("mmap");
}

void videodevice::startCapturing()
{
 struct v4l2_buffer buf;

 memset(&buf, 0, sizeof(buf));

 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 buf.memory = V4L2_MEMORY_MMAP;
 buf.index = 0;

 xioctl(fd, VIDIOC_QBUF, &buf);

 enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

 xioctl(fd, VIDIOC_STREAMON, &type);
}

int videodevice::readFrame(string file_name)
{
 struct v4l2_buffer buf;

 buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 buf.memory = V4L2_MEMORY_MMAP;

 if (xioctl(fd, VIDIOC_DQBUF, &buf) == EAGAIN)
 return 0;

 buffer *temp = devbuffer;

 FILE *out_file = fopen(file_name.c_str(),"w");

 fwrite(temp->start,temp->length,1,out_file);

 fclose(out_file);

 return 1;
}