static int video_prepare_capture(struct device *dev, int nbufs, unsigned int offset,
const char *filename)
{
unsigned int i;
int ret;
/* Allocate and map buffers. */
if ((ret = video_alloc_buffers(dev, nbufs, offset)) < 0)
return ret;
if (dev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
ret = video_load_test_pattern(dev, filename);
if (ret < 0)
return ret;
}
/* Queue the buffers. */
for (i = 0; i < dev->nbufs; ++i) {
ret = video_queue_buffer(dev, i);
if (ret < 0)
return ret;
}
return 0;
}
int video_prepare_capture(struct device *dev, int nbufs, unsigned int offset,
const char *filename, enum buffer_fill_mode fill)
{
unsigned int padding;
unsigned int i;
int ret;
/* Allocate and map buffers. */
padding = (fill & BUFFER_FILL_PADDING) ? 4096 : 0;
if ((ret = video_alloc_buffers(dev, nbufs, offset, padding)) < 0)
return ret;
if (dev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
ret = video_load_test_pattern(dev, filename);
if (ret < 0)
return ret;
}
/* Queue the buffers. */
for (i = 0; i < dev->nbufs; ++i) {
ret = video_queue_buffer(dev, i, fill);
if (ret < 0)
return ret;
}
return 0;
}
static int video_alloc_buffers(Uvc *dev)
{
struct v4l2_requestbuffers rb;
struct v4l2_buffer buf;
struct buffer *buffers;
unsigned int i;
int ret;
int nbufs = V4L_BUFFERS_DEFAULT;
memset(&rb, 0, sizeof(rb));
rb.count = nbufs;
rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
rb.memory = V4L2_MEMORY_MMAP;
ret = ioctl(dev->fd, VIDIOC_REQBUFS, &rb);
if (ret < 0) {
fprintf(stderr, "Failed request buffer\r\n");
return ret;
}
buffers = malloc(rb.count * sizeof buffers[0]);
if (buffers == NULL)
return -ENOMEM;
/* Map the buffers. */
for (i = 0; i < rb.count; ++i) {
memset(&buf, 0, sizeof buf);
buf.index = i;
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
ret = ioctl(dev->fd, VIDIOC_QUERYBUF, &buf);
if (ret < 0) {
fprintf(stderr, "Unable to query buffer %u (%d).\r\n", i, errno);
return ret;
}
//printf("length: %u offset: %u\n", buf.length, buf.m.offset);
buffers[i].mem = mmap(0, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, dev->fd, buf.m.offset);
if (buffers[i].mem == MAP_FAILED) {
fprintf(stderr, "Unable to map buffer %u (%d)\r\n", i, errno);
return -1;
}
buffers[i].size = buf.length;
//printf("Buffer %u mapped at address %p.\n", i, buffers[i].mem);
}
dev->buffers = buffers;
dev->nbufs = rb.count;
fprintf(stderr, "Queing buffers\r\n");
for(i = 0; i < rb.count; i++) {
ret = video_queue_buffer(dev, i);
if(ret < 0) return ret;
}
return 0;
}
static int video_do_capture(struct device *dev, unsigned int nframes,
unsigned int skip, unsigned int delay, const char *filename_prefix,
int do_requeue_last)
{
char *filename = NULL;
struct timeval start = { 0, 0 };
struct timeval end, ts;
struct v4l2_buffer buf;
unsigned int size;
unsigned int i;
FILE *file;
double bps;
double fps;
int ret;
if (filename_prefix != NULL) {
filename = malloc(strlen(filename_prefix) + 12);
if (filename == NULL)
return -ENOMEM;
}
/* Start streaming. */
video_enable(dev, 1);
size = 0;
for (i = 0; i < nframes; ++i) {
/* Dequeue a buffer. */
memset(&buf, 0, sizeof buf);
buf.type = dev->type;
buf.memory = dev->memtype;
ret = ioctl(dev->fd, VIDIOC_DQBUF, &buf);
if (ret < 0) {
if (errno != EIO) {
printf("Unable to dequeue buffer (%d).\n", errno);
goto done;
}
buf.type = dev->type;
buf.memory = dev->memtype;
if (dev->memtype == V4L2_MEMORY_USERPTR)
buf.m.userptr = (unsigned long)dev->buffers[i].mem;
}
if (dev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
dev->imagesize != 0 && buf.bytesused != dev->imagesize)
printf("Warning: bytes used %u != image size %u\n",
buf.bytesused, dev->imagesize);
size += buf.bytesused;
gettimeofday(&ts, NULL);
printf("%u (%u) [%c] %u %u bytes %ld.%06ld %ld.%06ld\n", i, buf.index,
(buf.flags & V4L2_BUF_FLAG_ERROR) ? 'E' : '-',
buf.sequence, buf.bytesused, buf.timestamp.tv_sec,
buf.timestamp.tv_usec, ts.tv_sec, ts.tv_usec);
if (i == 0)
start = ts;
/* Save the image. */
if (dev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && filename_prefix && !skip) {
sprintf(filename, "%s-%06u.bin", filename_prefix, i);
file = fopen(filename, "wb");
if (file != NULL) {
ret = fwrite(dev->buffers[buf.index].mem, buf.bytesused, 1, file);
fclose(file);
}
}
if (skip)
--skip;
/* Requeue the buffer. */
if (delay > 0)
usleep(delay * 1000);
fflush(stdout);
if (i == nframes - dev->nbufs && !do_requeue_last)
continue;
ret = video_queue_buffer(dev, buf.index);
if (ret < 0) {
printf("Unable to requeue buffer (%d).\n", errno);
goto done;
}
}
gettimeofday(&end, NULL);
/* Stop streaming. */
video_enable(dev, 0);
if (nframes == 0) {
printf("No frames captured.\n");
goto done;
}
if (end.tv_sec == start.tv_sec && end.tv_usec == start.tv_usec)
goto done;
end.tv_sec -= start.tv_sec;
end.tv_usec -= start.tv_usec;
if (end.tv_usec < 0) {
end.tv_sec--;
end.tv_usec += 1000000;
}
bps = size/(end.tv_usec+1000000.0*end.tv_sec)*1000000.0;
fps = (i-1)/(end.tv_usec+1000000.0*end.tv_sec)*1000000.0;
printf("Captured %u frames in %lu.%06lu seconds (%f fps, %f B/s).\n",
i-1, end.tv_sec, end.tv_usec, fps, bps);
done:
free(filename);
return video_free_buffers(dev);
}
static void uvc_drv_input(ErlDrvData handle, ErlDrvEvent io_event)
{
Uvc* d = (Uvc*) handle;
struct v4l2_buffer buf;
memset(&buf, 0, sizeof buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
int ret = ioctl(d->fd, VIDIOC_DQBUF, &buf);
if(ret < 0) {
driver_failure_posix(d->port, errno);
return;
}
ErlDrvBinary* bin;
size_t len;
if(d->pixelformat == V4L2_PIX_FMT_YUYV) {
len = d->width*d->height*3/2;
bin = driver_alloc_binary(len);
if(!d->scale_ctx) d->scale_ctx = sws_getContext(
d->width, d->height, PIX_FMT_YUYV422,
d->width, d->height, PIX_FMT_YUV420P,
SWS_FAST_BILINEAR, NULL, NULL, NULL
);
int linesize[4] = {d->width*2, 0, 0, 0};
uint8_t *src[4] = {(uint8_t *)d->buffers[buf.index].mem, 0, 0, 0};
int stride_size = d->width*d->height;
uint8_t *plane[4] = {(uint8_t *)bin->orig_bytes, (uint8_t *)bin->orig_bytes+stride_size, (uint8_t *)bin->orig_bytes+stride_size+stride_size/4, NULL};
int stride[4] = {d->width, d->width/2, d->width/2, 0};
sws_scale(d->scale_ctx, (const uint8_t * const*)src, linesize, 0, d->height, plane, stride);
} else {
bin = driver_alloc_binary(buf.bytesused + 1024);
len = add_huffman((uint8_t *)bin->orig_bytes, (uint8_t *)d->buffers[buf.index].mem, buf.bytesused);
}
ErlDrvUInt64 pts = buf.timestamp.tv_sec * 1000 + buf.timestamp.tv_usec / 1000;
ErlDrvTermData reply[] = {
ERL_DRV_ATOM, driver_mk_atom("uvc"),
ERL_DRV_PORT, driver_mk_port(d->port),
ERL_DRV_ATOM, driver_mk_atom(d->pixelformat == V4L2_PIX_FMT_YUYV ? "yuv" : "jpeg"),
ERL_DRV_UINT64, &pts,
ERL_DRV_BINARY, (ErlDrvTermData)bin, (ErlDrvTermData)len, 0,
ERL_DRV_TUPLE, 5
};
// fprintf(stderr, "Event in uvc: %lu %u %u\r\n", len, (unsigned)buf.timestamp.tv_sec, (unsigned)buf.timestamp.tv_usec);
driver_output_term(d->port, reply, sizeof(reply) / sizeof(reply[0]));
driver_free_binary(bin);
ret = video_queue_buffer(d, buf.index);
if(ret < 0) {
driver_failure_posix(d->port, errno);
return;
}
}
int video_do_capture(struct device *dev, unsigned int nframes,
unsigned int skip, unsigned int delay, const char *pattern,
int do_requeue_last, enum buffer_fill_mode fill)
{
struct timespec start;
struct timeval last;
struct timespec ts;
struct v4l2_buffer buf;
unsigned int size;
unsigned int i;
double bps;
double fps;
int ret;
/* Start streaming. */
ret = video_enable(dev, 1);
if (ret < 0)
goto done;
size = 0;
clock_gettime(CLOCK_MONOTONIC, &start);
last.tv_sec = start.tv_sec;
last.tv_usec = start.tv_nsec / 1000;
for (i = 0; i < nframes; ++i) {
/* Dequeue a buffer. */
memset(&buf, 0, sizeof buf);
buf.type = dev->type;
buf.memory = dev->memtype;
ret = ioctl(dev->fd, VIDIOC_DQBUF, &buf);
if (ret < 0) {
if (errno != EIO) {
printf("Unable to dequeue buffer: %s (%d).\n",
strerror(errno), errno);
goto done;
}
buf.type = dev->type;
buf.memory = dev->memtype;
if (dev->memtype == V4L2_MEMORY_USERPTR)
buf.m.userptr = (unsigned long)dev->buffers[i].mem;
}
if (dev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
dev->imagesize != 0 && buf.bytesused != dev->imagesize)
printf("Warning: bytes used %u != image size %u\n",
buf.bytesused, dev->imagesize);
if (dev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
video_verify_buffer(dev, buf.index);
size += buf.bytesused;
fps = (buf.timestamp.tv_sec - last.tv_sec) * 1000000
+ buf.timestamp.tv_usec - last.tv_usec;
fps = fps ? 1000000.0 / fps : 0.0;
clock_gettime(CLOCK_MONOTONIC, &ts);
printf("%u (%u) [%c] %u %u bytes %ld.%06ld %ld.%06ld %.3f fps\n", i, buf.index,
(buf.flags & V4L2_BUF_FLAG_ERROR) ? 'E' : '-',
buf.sequence, buf.bytesused, buf.timestamp.tv_sec,
buf.timestamp.tv_usec, ts.tv_sec, ts.tv_nsec/1000, fps);
last = buf.timestamp;
/* Save the image. */
if (dev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pattern && !skip)
video_save_image(dev, &buf, pattern, i);
if (skip)
--skip;
/* Requeue the buffer. */
if (delay > 0)
usleep(delay * 1000);
fflush(stdout);
if (i == nframes - dev->nbufs && !do_requeue_last)
continue;
ret = video_queue_buffer(dev, buf.index, fill);
if (ret < 0) {
printf("Unable to requeue buffer: %s (%d).\n",
strerror(errno), errno);
goto done;
}
}
/* Stop streaming. */
video_enable(dev, 0);
if (nframes == 0) {
printf("No frames captured.\n");
goto done;
}
if (ts.tv_sec == start.tv_sec && ts.tv_nsec == start.tv_nsec)
goto done;
ts.tv_sec -= start.tv_sec;
ts.tv_nsec -= start.tv_nsec;
if (ts.tv_nsec < 0) {
ts.tv_sec--;
ts.tv_nsec += 1000000000;
}
bps = size/(ts.tv_nsec/1000.0+1000000.0*ts.tv_sec)*1000000.0;
fps = i/(ts.tv_nsec/1000.0+1000000.0*ts.tv_sec)*1000000.0;
printf("Captured %u frames in %lu.%06lu seconds (%f fps, %f B/s).\n",
i, ts.tv_sec, ts.tv_nsec/1000, fps, bps);
done:
return video_free_buffers(dev);
}
