152 lines
3.6 KiB
C
152 lines
3.6 KiB
C
#include <libmegapixels.h>
|
|
#include <stdio.h>
|
|
#include <limits.h>
|
|
#include <string.h>
|
|
#include <linux/videodev2.h>
|
|
#include <sys/ioctl.h>
|
|
#include <stdlib.h>
|
|
#include <sys/types.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <sys/mman.h>
|
|
|
|
struct buffer {
|
|
void *start;
|
|
size_t length;
|
|
};
|
|
struct buffer *buffers;
|
|
|
|
int
|
|
xioctl(int fd, int request, void *arg)
|
|
{
|
|
int r;
|
|
do {
|
|
r = ioctl(fd, request, arg);
|
|
} while (r == -1 && errno == EINTR);
|
|
return r;
|
|
}
|
|
|
|
int
|
|
main()
|
|
{
|
|
char configpath[PATH_MAX];
|
|
int ret = libmegapixels_find_config(configpath);
|
|
libmegapixels_devconfig *config = {0};
|
|
if (ret) {
|
|
printf("Using config: %s\n", configpath);
|
|
libmegapixels_load_file(&config, configpath);
|
|
} else {
|
|
fprintf(stderr, "No config found for this device\n");
|
|
return 1;
|
|
}
|
|
|
|
if (config->count == 0) {
|
|
fprintf(stderr, "No valid camera configuration\n");
|
|
return 1;
|
|
}
|
|
|
|
libmegapixels_camera *camera = config->cameras[0];
|
|
if (libmegapixels_open(camera) != 0) {
|
|
fprintf(stderr, "Could not open default camera\n");
|
|
return 1;
|
|
}
|
|
|
|
libmegapixels_mode *mode = camera->modes[0];
|
|
unsigned int frame_size = libmegapixels_select_mode(camera, mode);
|
|
if (frame_size == 0) {
|
|
fprintf(stderr, "Could not select mode\n");
|
|
return 1;
|
|
}
|
|
|
|
// Do the reqular V4L2 stuff to get a frame
|
|
struct v4l2_capability cap;
|
|
if (ioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) != 0) {
|
|
fprintf(stderr, "VIDIOC_QUERYCAP failed\n");
|
|
return 1;
|
|
}
|
|
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
|
|
fprintf(stderr, "Device does not support streaming i/o\n");
|
|
return 1;
|
|
}
|
|
|
|
struct v4l2_requestbuffers req = {0};
|
|
req.count = 4;
|
|
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
req.memory = V4L2_MEMORY_MMAP;
|
|
if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) {
|
|
fprintf(stderr, "VIDIOC_REQBUFS failed\n");
|
|
return 1;
|
|
}
|
|
buffers = calloc(req.count, sizeof(*buffers));
|
|
for (int i = 0; i < req.count; i++) {
|
|
struct v4l2_buffer buf = {0};
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
buf.index = i;
|
|
|
|
if (xioctl(camera->video_fd, VIDIOC_QUERYBUF, &buf) == -1) {
|
|
fprintf(stderr, "VIDIOC_QUERYBUF failed\n");
|
|
return 1;
|
|
}
|
|
|
|
buffers[i].length = buf.length;
|
|
buffers[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, camera->video_fd, buf.m.offset);
|
|
if (buffers[i].start == MAP_FAILED) {
|
|
fprintf(stderr, "mmap() failed\n");
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < req.count; i++) {
|
|
struct v4l2_buffer qbuf = {0};
|
|
qbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
qbuf.memory = V4L2_MEMORY_MMAP;
|
|
qbuf.index = i;
|
|
|
|
if (xioctl(camera->video_fd, VIDIOC_QBUF, &qbuf) == -1) {
|
|
fprintf(stderr, "VIDIOC_QBUF failed\n");
|
|
return 1;
|
|
}
|
|
}
|
|
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
if (xioctl(camera->video_fd, VIDIOC_STREAMON, &type) == -1) {
|
|
fprintf(stderr, "VIDIOC_STREAMON failed\n");
|
|
return 1;
|
|
}
|
|
|
|
|
|
int count = 5;
|
|
while (count-- > 0) {
|
|
while (1) {
|
|
fd_set(fds);
|
|
FD_ZERO(&fds);
|
|
FD_SET(camera->video_fd, &fds);
|
|
|
|
int sr = select(FD_SETSIZE, &fds, NULL, NULL, NULL);
|
|
if (sr == -1) {
|
|
if (errno == EINTR) {
|
|
count++;
|
|
continue;
|
|
}
|
|
fprintf(stderr, "select() failed: %s\n", strerror(errno));
|
|
return 1;
|
|
}
|
|
|
|
|
|
struct v4l2_buffer buf = {0};
|
|
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
|
buf.memory = V4L2_MEMORY_MMAP;
|
|
if (xioctl(camera->video_fd, VIDIOC_DQBUF, &buf) == -1) {
|
|
fprintf(stderr, "VIDIOC_DQBUF failed\n");
|
|
return 1;
|
|
}
|
|
fprintf(stderr, "GOT FRAME!\n");
|
|
if (xioctl(camera->video_fd, VIDIOC_QBUF, &buf) == -1) {
|
|
fprintf(stderr, "VIDIOC_DQBUF failed\n");
|
|
return 1;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
return 0;
|
|
} |