Created
January 14, 2016 18:26
-
-
Save kevinmehall/17f2f1ad113f4b4349dc to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
Copyright (C) 2015 3D Robotics | |
Permission is hereby granted, free of charge, to any | |
person obtaining a copy of this software and associated | |
documentation files (the "Software"), to deal in the | |
Software without restriction, including without | |
limitation the rights to use, copy, modify, merge, | |
publish, distribute, sublicense, and/or sell copies of | |
the Software, and to permit persons to whom the Software | |
is furnished to do so, subject to the following | |
conditions: | |
The above copyright notice and this permission notice | |
shall be included in all copies or substantial portions | |
of the Software. | |
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF | |
ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED | |
TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A | |
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT | |
SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY | |
CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION | |
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR | |
IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER | |
DEALINGS IN THE SOFTWARE. | |
*/ | |
// Based on https://linuxtv.org/downloads/v4l-dvb-apis/capture-example.html | |
#include <stdio.h> | |
#include <stdlib.h> | |
#include <string.h> | |
#include <assert.h> | |
#include <stdint.h> | |
#include <fcntl.h> | |
#include <unistd.h> | |
#include <errno.h> | |
#include <sys/stat.h> | |
#include <sys/types.h> | |
#include <sys/time.h> | |
#include <sys/mman.h> | |
#include <sys/ioctl.h> | |
#include <linux/videodev2.h> | |
#include "solocam.h" | |
#define CLEAR(x) memset(&(x), 0, sizeof(x)) | |
#define MAX_BUFFERS 8 | |
struct solocam_ctx { | |
int fd; | |
unsigned num_buffers; | |
solocam_buf buffers[MAX_BUFFERS]; | |
struct v4l2_format fmt; | |
}; | |
static void logerr(const char *s) { | |
fprintf(stderr, "%s error %d, %s\n", s, errno, strerror(errno)); | |
} | |
static int xioctl(int fh, int request, void *arg) { | |
int r; | |
do { | |
r = ioctl(fh, request, arg); | |
} while (-1 == r && EINTR == errno); | |
return r; | |
} | |
int solocam_open_hdmi(struct solocam_ctx** ctxp) { | |
struct stat st; | |
const char* dev_name = "/dev/video0"; | |
if (-1 == stat(dev_name, &st)) { | |
fprintf(stderr, "Cannot identify '%s': %d, %s\n", | |
dev_name, errno, strerror(errno)); | |
return EXIT_FAILURE; | |
} | |
if (!S_ISCHR(st.st_mode)) { | |
fprintf(stderr, "%s is no device\n", dev_name); | |
return EXIT_FAILURE; | |
} | |
int fd = open(dev_name, O_RDWR /* required */ /*| O_NONBLOCK*/, 0); | |
if (-1 == fd) { | |
fprintf(stderr, "Cannot open '%s': %d, %s\n", | |
dev_name, errno, strerror(errno)); | |
return EXIT_FAILURE; | |
} | |
struct v4l2_capability cap; | |
if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap)) { | |
if (EINVAL == errno) { | |
fprintf(stderr, "%s is no V4L2 device\n", | |
dev_name); | |
exit(EXIT_FAILURE); | |
} else { | |
logerr("VIDIOC_QUERYCAP"); | |
return errno; | |
} | |
} | |
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { | |
fprintf(stderr, "%s is no video capture device\n", | |
dev_name); | |
return EXIT_FAILURE; | |
} | |
if (!(cap.capabilities & V4L2_CAP_STREAMING)) { | |
fprintf(stderr, "%s does not support streaming i/o\n", | |
dev_name); | |
return EXIT_FAILURE; | |
} | |
struct v4l2_cropcap cropcap; | |
struct v4l2_crop crop; | |
CLEAR(cropcap); | |
cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
if (0 == xioctl(fd, VIDIOC_CROPCAP, &cropcap)) { | |
crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
crop.c = cropcap.defrect; /* reset to default */ | |
if (-1 == xioctl(fd, VIDIOC_S_CROP, &crop)) { | |
switch (errno) { | |
case EINVAL: | |
/* Cropping not supported. */ | |
break; | |
default: | |
/* Errors ignored. */ | |
break; | |
} | |
} | |
} else { | |
/* Errors ignored. */ | |
} | |
int s_input = 1; | |
if (-1 == xioctl(fd, VIDIOC_S_INPUT, &s_input)) { | |
logerr("VIDIOC_S_INPUT"); | |
return errno; | |
} | |
solocam_ctx* ctx = calloc(1, sizeof(*ctx)); | |
ctx->fd = fd; | |
*ctxp = ctx; | |
return 0; | |
} | |
int solocam_crop(struct solocam_ctx* ctx, int x, int y, int width, int height) { | |
struct v4l2_crop crop; | |
CLEAR(crop); | |
crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
crop.c.left = x; | |
crop.c.top = y; | |
crop.c.width = width; | |
crop.c.height = height; | |
if (-1 == xioctl(ctx->fd, VIDIOC_S_CROP, &crop)) { | |
logerr("VIDIOC_S_CROP"); | |
return errno; | |
} | |
return 0; | |
} | |
int solocam_get_size(struct solocam_ctx* ctx, int* widthp, int* heightp) { | |
*widthp = ctx->fmt.fmt.pix.width; | |
*heightp = ctx->fmt.fmt.pix.height; | |
return 0; | |
} | |
/// Grayscale = Y plane of YUV420 | |
int solocam_set_format_720p60_grayscale(struct solocam_ctx* ctx) { | |
struct v4l2_streamparm streamparm; | |
streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
streamparm.parm.capture.capturemode = 4; | |
streamparm.parm.capture.timeperframe.denominator = 60; | |
streamparm.parm.capture.timeperframe.numerator = 1; | |
if (-1 == xioctl(ctx->fd, VIDIOC_S_PARM, &streamparm)) { | |
logerr("VIDIOC_S_PARM"); | |
return errno; | |
} | |
CLEAR(ctx->fmt); | |
ctx->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
ctx->fmt.fmt.pix.width = 1280; | |
ctx->fmt.fmt.pix.height = 720; | |
ctx->fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420; | |
ctx->fmt.fmt.pix.field = V4L2_FIELD_ANY; | |
if (-1 == xioctl(ctx->fd, VIDIOC_S_FMT, &ctx->fmt)) { | |
logerr("VIDIOC_S_FMT"); | |
return errno; | |
} | |
/* | |
if (-1 == xioctl(ctx->fd, VIDIOC_G_FMT, &ctx->fmt)){ | |
logerr("VIDIOC_G_FMT"); | |
return errno; | |
} | |
*/ | |
/* Buggy driver paranoia. */ | |
int min = ctx->fmt.fmt.pix.width * 2; | |
if (ctx->fmt.fmt.pix.bytesperline < min) | |
ctx->fmt.fmt.pix.bytesperline = min; | |
min = ctx->fmt.fmt.pix.bytesperline * ctx->fmt.fmt.pix.height; | |
if (ctx->fmt.fmt.pix.sizeimage < min) | |
ctx->fmt.fmt.pix.sizeimage = min; | |
return 0; | |
} | |
int solocam_start(struct solocam_ctx* ctx) { | |
struct v4l2_requestbuffers req; | |
CLEAR(req); | |
req.count = MAX_BUFFERS; | |
req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
req.memory = V4L2_MEMORY_MMAP; | |
if (-1 == xioctl(ctx->fd, VIDIOC_REQBUFS, &req)) { | |
if (EINVAL == errno) { | |
fprintf(stderr, "Device does not support memory mapping\n"); | |
return EXIT_FAILURE; | |
} else { | |
logerr("VIDIOC_REQBUFS"); | |
return errno; | |
} | |
} | |
if (req.count < 2) { | |
fprintf(stderr, "Insufficient buffer memory on device\n"); | |
return EXIT_FAILURE; | |
} | |
ctx->num_buffers = req.count; | |
for (int n_buffers = 0; n_buffers < ctx->num_buffers; ++n_buffers) { | |
struct v4l2_buffer buf; | |
CLEAR(buf); | |
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
buf.memory = V4L2_MEMORY_MMAP; | |
buf.index = n_buffers; | |
if (-1 == xioctl(ctx->fd, VIDIOC_QUERYBUF, &buf)) { | |
logerr("VIDIOC_QUERYBUF"); | |
return errno; | |
} | |
ctx->buffers[n_buffers].id = n_buffers; | |
ctx->buffers[n_buffers].length = buf.length; | |
ctx->buffers[n_buffers].data = | |
mmap(NULL, | |
buf.length, | |
PROT_READ | PROT_WRITE, | |
MAP_SHARED, | |
ctx->fd, buf.m.offset); | |
if (MAP_FAILED == ctx->buffers[n_buffers].data) { | |
logerr("mmap"); | |
return errno; | |
} | |
} | |
for (int i = 0; i < ctx->num_buffers; ++i) { | |
struct v4l2_buffer buf; | |
CLEAR(buf); | |
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
buf.memory = V4L2_MEMORY_MMAP; | |
buf.index = i; | |
if (-1 == xioctl(ctx->fd, VIDIOC_QBUF, &buf)) { | |
logerr("VIDIOC_QBUF (start)"); | |
return errno; | |
} | |
} | |
unsigned type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
if (-1 == xioctl(ctx->fd, VIDIOC_STREAMON, &type)) { | |
logerr("VIDIOC_STREAMON"); | |
return errno; | |
} | |
return 0; | |
} | |
int solocam_stop(struct solocam_ctx* ctx) { | |
enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
if (-1 == xioctl(ctx->fd, VIDIOC_STREAMOFF, &type)) { | |
logerr("VIDIOC_STREAMOFF"); | |
} | |
return 0; | |
} | |
int solocam_read_frame(struct solocam_ctx* ctx, solocam_buf** bufp) { | |
struct v4l2_buffer buf; | |
CLEAR(buf); | |
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
buf.memory = V4L2_MEMORY_MMAP; | |
if (-1 == xioctl(ctx->fd, VIDIOC_DQBUF, &buf)) { | |
logerr("VIDIOC_DQBUF"); | |
return errno; | |
} | |
assert(buf.index < ctx->num_buffers); | |
ctx->buffers[buf.index].used = buf.bytesused; | |
*bufp = &ctx->buffers[buf.index]; | |
return 0; | |
} | |
int solocam_free_frame(struct solocam_ctx* ctx, solocam_buf* buf) { | |
struct v4l2_buffer vbuf; | |
vbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
vbuf.memory = V4L2_MEMORY_MMAP; | |
vbuf.index = buf->id; | |
if (-1 == xioctl(ctx->fd, VIDIOC_QBUF, &vbuf)) { | |
logerr("VIDIOC_QBUF (free)"); | |
return errno; | |
} | |
return 0; | |
} | |
int solocam_close(struct solocam_ctx* ctx) { | |
close(ctx->fd); | |
free(ctx); | |
return 0; | |
} | |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <stdint.h> | |
#include <stdlib.h> | |
struct solocam_ctx; | |
typedef struct solocam_ctx solocam_ctx; | |
typedef struct solocam_buf { | |
int id; | |
uint8_t *data; | |
size_t length; | |
size_t used; | |
} solocam_buf; | |
int solocam_open_hdmi(solocam_ctx** ctx); | |
int solocam_crop(solocam_ctx* ctx, int x, int y, int width, int height); | |
int solocam_get_size(solocam_ctx* ctx, int* widthp, int* heightp); | |
// YUV420, use Y plane for grayscale | |
int solocam_set_format_720p60_grayscale(solocam_ctx* ctx); | |
int solocam_start(solocam_ctx* ctx); | |
int solocam_stop(solocam_ctx* ctx); | |
int solocam_read_frame(solocam_ctx* ctx, solocam_buf** bufptr); | |
int solocam_free_frame(solocam_ctx* ctx, solocam_buf* ptr); | |
int solocam_close(solocam_ctx* ctx); |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment