void control_readed(struct vdIn *vd,struct v4l2_queryctrl *ctrl, globals *pglobal)
{
struct v4l2_control c;
c.id = ctrl->id;
if(IOCTL_VIDEO(vd->fd, VIDIOC_G_CTRL, &c) != -1) {
pglobal->in.in_parameters =
(input_control*)realloc(
pglobal->in.in_parameters,
(pglobal->in.parametercount + 1) * sizeof(input_control));
memcpy(&pglobal->in.in_parameters[pglobal->in.parametercount].ctrl, ctrl, sizeof(struct v4l2_queryctrl));
pglobal->in.in_parameters[pglobal->in.parametercount].value = c.value;
if (ctrl->type == V4L2_CTRL_TYPE_MENU) {
pglobal->in.in_parameters[pglobal->in.parametercount].menuitems =
(struct v4l2_querymenu*)malloc((ctrl->maximum+1) * sizeof(struct v4l2_querymenu));
int i;
for(i=ctrl->minimum; i<=ctrl->maximum; i++) {
struct v4l2_querymenu qm;
qm.id = ctrl->id;
qm.index = i;
if(IOCTL_VIDEO(vd->fd, VIDIOC_QUERYMENU, &qm) == 0) {
memcpy(&pglobal->in.in_parameters[pglobal->in.parametercount].menuitems[i], &qm, sizeof(struct v4l2_querymenu));
DBG("Menu item %d: %s\n", qm.index, qm.name);
} else {
DBG("Unable to get menu item for %s, index=%d\n", ctrl->name, qm.index);
}
}
} else {
pglobal->in.in_parameters[pglobal->in.parametercount].menuitems = NULL;
}
DBG("V4L2 parameter found: %s value %d \n", ctrl->name, c.value);
pglobal->in.parametercount++;
} else {
DBG("Unable to get the value of %s", ctrl->name);
}
};
void initDynCtrls(int dev) {
int i, err;
/* try to add all controls listed above */
for ( i=0; i<LENGTH_OF(xu_ctrls); i++ ) {
DBG("adding control for %d\n", i);
errno=0;
if ( (err=IOCTL_VIDEO(dev, UVCIOC_CTRL_ADD, &xu_ctrls[i])) < 0 ) {
if ( errno != EEXIST ) {
DBG("uvcioc ctrl add error: errno=%d (retval=%d)\n", errno, err);
} else {
DBG("control %d already exists\n", i);
}
}
}
/* after adding the controls, add the mapping now */
for ( i=0; i<LENGTH_OF(xu_mappings); i++ ) {
DBG("mapping controls for %s\n", xu_mappings[i].name);
errno=0;
if ((err=IOCTL_VIDEO(dev, UVCIOC_CTRL_MAP, &xu_mappings[i])) < 0) {
if (errno != EEXIST) {
DBG("uvcioc ctrl map error: errno=%d (retval=%d)\n", errno, err);
} else {
DBG("mapping %d already exists\n", i);
}
}
}
}
int setResolution(struct vdIn *vd, int width, int height)
{
int ret;
DBG("setResolution(%d, %d)\n", width, height);
vd->streamingState = STREAMING_PAUSED;
if (video_disable(vd, STREAMING_PAUSED) == 0) {
ret = IOCTL_VIDEO(vd->fd, VIDIOC_G_FMT, &vd->fmt);
if (ret != 0) {
DBG("Unable to get current format\n");
return ret;
} else {
DBG("Current size: %d, %d)\n", vd->fmt.fmt.pix.width, vd->fmt.fmt.pix.height);
}
vd->fmt.fmt.pix.width = width;
vd->fmt.fmt.pix.height = height;
ret = IOCTL_VIDEO(vd->fd, VIDIOC_S_FMT, &vd->fmt);
if (ret != 0) {
DBG("Unable to set the new format code: %d errno: %d\n", ret, errno);
if (errno == EBUSY)
DBG("EBUSY: IO is in progress\n");
} else {
DBG("New resolution is successfully applied\n");
}
if (video_enable(vd) == 0) {
DBG("Streaming on again\n");
memset(&vd->rb, 0, sizeof(struct v4l2_requestbuffers));
vd->rb.count = NB_BUFFER;
vd->rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd->rb.memory = V4L2_MEMORY_MMAP;
ret = IOCTL_VIDEO(vd->fd, VIDIOC_REQBUFS, &vd->rb);
if (ret < 0) {
perror("Unable to reallocate buffers");
}
} else {
DBG("Unable to reenable streaming\n");
return -1;
}
} else {
DBG("Unable to disable streaming\n");
return -1;
}
return ret;
}
/* return >= 0 ok otherwhise -1 */
static int isv4l2Control(struct vdIn *vd, int control, struct v4l2_queryctrl *queryctrl) {
int err =0;
queryctrl->id = control;
if ((err= IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, queryctrl)) < 0) {
//fprintf(stderr, "ioctl querycontrol error %d \n",errno);
return -1;
}
if (queryctrl->flags & V4L2_CTRL_FLAG_DISABLED) {
//fprintf(stderr, "control %s disabled \n", (char *) queryctrl->name);
return -1;
}
if (queryctrl->type & V4L2_CTRL_TYPE_BOOLEAN) {
return 1;
}
if (queryctrl->type & V4L2_CTRL_TYPE_INTEGER) {
return 0;
}
fprintf(stderr, "contol %s unsupported \n", (char *) queryctrl->name);
return -1;
}
/*
SRC: https://lists.berlios.de/pipermail/linux-uvc-devel/2007-July/001888.html
- dev: the device file descriptor
- pan: pan angle in 1/64th of degree
- tilt: tilt angle in 1/64th of degree
- reset: set to 1 to reset pan/tilt to the device origin, set to 0 otherwise
*/
int uvcPanTilt(int dev, int pan, int tilt, int reset) {
struct v4l2_ext_control xctrls[2];
struct v4l2_ext_controls ctrls;
if (reset) {
xctrls[0].id = V4L2_CID_PANTILT_RESET_LOGITECH;
xctrls[0].value = 3;
ctrls.count = 1;
ctrls.controls = xctrls;
} else {
xctrls[0].id = V4L2_CID_PAN_RELATIVE_LOGITECH;
xctrls[0].value = pan;
xctrls[1].id = V4L2_CID_TILT_RELATIVE_LOGITECH;
xctrls[1].value = tilt;
ctrls.count = 2;
ctrls.controls = xctrls;
}
if ( IOCTL_VIDEO(dev, VIDIOC_S_EXT_CTRLS, &ctrls) < 0 ) {
return -1;
}
return 0;
}
/******************************************************************************
Description.: process commands, allows to set v4l2 controls
Input Value.: * control specifies the selected v4l2 control's id
see struct v4l2_queryctr in the videodev2.h
* value is used for control that make use of a parameter.
Return Value: depends in the command, for most cases 0 means no errors and
-1 signals an error. This is just rule of thumb, not more!
******************************************************************************/
int input_cmd(int plugin_number, unsigned int control_id, unsigned int group, int value)
{
int ret = -1;
int i = 0;
DBG("Requested cmd (id: %d) for the %d plugin. Group: %d value: %d\n", control_id, plugin_number, group, value);
switch(group) {
case IN_CMD_GENERIC: {
int i;
for (i = 0; i<pglobal->in[plugin_number].parametercount; i++) {
if ((pglobal->in[plugin_number].in_parameters[i].ctrl.id == control_id) &&
(pglobal->in[plugin_number].in_parameters[i].group == IN_CMD_GENERIC)){
DBG("Generic control found (id: %d): %s\n", control_id, pglobal->in[plugin_number].in_parameters[i].ctrl.name);
DBG("New %s value: %d\n", pglobal->in[plugin_number].in_parameters[i].ctrl.name, value);
return 0;
}
}
DBG("Requested generic control (%d) did not found\n", control_id);
return -1;
} break;
case IN_CMD_V4L2: {
ret = v4l2SetControl(cams[plugin_number].videoIn, control_id, value, plugin_number, pglobal);
if(ret == 0) {
pglobal->in[plugin_number].in_parameters[i].value = value;
} else {
DBG("v4l2SetControl failed: %d\n", ret);
}
return ret;
} break;
case IN_CMD_RESOLUTION: {
// the value points to the current formats nth resolution
if(value > (pglobal->in[plugin_number].in_formats[pglobal->in[plugin_number].currentFormat].resolutionCount - 1)) {
DBG("The value is out of range");
return -1;
}
int height = pglobal->in[plugin_number].in_formats[pglobal->in[plugin_number].currentFormat].supportedResolutions[value].height;
int width = pglobal->in[plugin_number].in_formats[pglobal->in[plugin_number].currentFormat].supportedResolutions[value].width;
ret = setResolution(cams[plugin_number].videoIn, width, height);
if(ret == 0) {
pglobal->in[plugin_number].in_formats[pglobal->in[plugin_number].currentFormat].currentResolution = value;
}
return ret;
} break;
case IN_CMD_JPEG_QUALITY:
if((value >= 0) && (value < 101)) {
pglobal->in[plugin_number].jpegcomp.quality = value;
if(IOCTL_VIDEO(cams[plugin_number].videoIn->fd, VIDIOC_S_JPEGCOMP, &pglobal->in[plugin_number].jpegcomp) != EINVAL) {
DBG("JPEG quality is set to %d\n", value);
ret = 0;
} else {
DBG("Setting the JPEG quality is not supported\n");
}
} else {
DBG("Quality is out of range\n");
}
break;
}
return ret;
}
/* ioctl with a number of retries in the case of failure
* args:
* fd - device descriptor
* IOCTL_X - ioctl reference
* arg - pointer to ioctl data
* returns - ioctl result
*/
int xioctl(int fd, int IOCTL_X, void *arg)
{
int ret = 0;
int tries = IOCTL_RETRY;
do {
ret = IOCTL_VIDEO(fd, IOCTL_X, arg);
} while(ret && tries-- &&
((errno == EINTR) || (errno == EAGAIN) || (errno == ETIMEDOUT)));
if(ret && (tries <= 0)) fprintf(stderr, "ioctl (%i) retried %i times - giving up: %s)\n", IOCTL_X, IOCTL_RETRY, strerror(errno));
return (ret);
}
static int video_enable(struct vdIn *vd)
{
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
int ret;
ret = IOCTL_VIDEO(vd->fd, VIDIOC_STREAMON, &type);
if (ret < 0) {
perror("Unable to start capture");
return ret;
}
vd->streamingState = STREAMING_ON;
return 0;
}
static int video_disable(struct vdIn *vd, streaming_state disabledState)
{
int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
int ret;
DBG("STopping capture\n");
ret = IOCTL_VIDEO(vd->fd, VIDIOC_STREAMOFF, &type);
if (ret != 0) {
perror("Unable to stop capture");
return ret;
}
DBG("STopping capture done\n");
vd->streamingState = disabledState;
return 0;
}
int v4l2GetControl(struct vdIn *vd, int control) {
struct v4l2_queryctrl queryctrl;
struct v4l2_control control_s;
int err;
if ((err = isv4l2Control(vd, control, &queryctrl)) < 0) {
return -1;
}
control_s.id = control;
if ((err = IOCTL_VIDEO(vd->fd, VIDIOC_G_CTRL, &control_s)) < 0) {
return -1;
}
return control_s.value;
}
static int xioctl(int fd, int IOCTL_X, void *arg) {
int ret = 0;
int tries = IOCTL_RETRY;
do {
ret = IOCTL_VIDEO(fd, IOCTL_X, arg);
} while(ret && tries-- &&
((errno == EINTR) || (errno == EAGAIN) || (errno == ETIMEDOUT)));
if (ret && tries <= 0) {
log_itf(LOG_ERROR, "ioctl (%x) retried %i times - giving up: %s.", IOCTL_X, IOCTL_RETRY, strerror(errno));
user_panic("ioctl error.");
}
return (ret);
}
int v4l2ResetControl(struct vdIn *vd, int control) {
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int val_def;
int err;
if (isv4l2Control(vd, control, &queryctrl) < 0)
return -1;
val_def = queryctrl.default_value;
control_s.id = control;
control_s.value = val_def;
if ((err = IOCTL_VIDEO(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
return -1;
}
return 0;
};
int v4l2SetControl(struct vdIn *vd, int control, int value) {
struct v4l2_control control_s;
struct v4l2_queryctrl queryctrl;
int min, max, step, val_def;
int err;
if (isv4l2Control(vd, control, &queryctrl) < 0)
return -1;
min = queryctrl.minimum;
max = queryctrl.maximum;
step = queryctrl.step;
val_def = queryctrl.default_value;
if ((value >= min) && (value <= max)) {
control_s.id = control;
control_s.value = value;
if ((err = IOCTL_VIDEO(vd->fd, VIDIOC_S_CTRL, &control_s)) < 0) {
return -1;
}
}
return 0;
}
void enumerateControls(struct vdIn *vd, globals *pglobal, int id)
{
// enumerating v4l2 controls
struct v4l2_queryctrl ctrl;
pglobal->in[id].parametercount = 0;
pglobal->in[id].in_parameters = NULL;
/* Enumerate the v4l2 controls
Try the extended control API first */
#ifdef V4L2_CTRL_FLAG_NEXT_CTRL
DBG("V4L2 API's V4L2_CTRL_FLAG_NEXT_CTRL is supported\n");
ctrl.id = V4L2_CTRL_FLAG_NEXT_CTRL;
if(0 == IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl)) {
do {
control_readed(vd, &ctrl, pglobal, id);
ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
} while(0 == IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl));
// note: use simple ioctl or v4l2_ioctl instead of the xioctl
} else
#endif
{
DBG("V4L2 API's V4L2_CTRL_FLAG_NEXT_CTRL is NOT supported\n");
/* Fall back on the standard API */
/* Check all the standard controls */
int i;
for(i = V4L2_CID_BASE; i < V4L2_CID_LASTP1; i++) {
ctrl.id = i;
if(IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl) == 0) {
control_readed(vd, &ctrl, pglobal, id);
}
}
/* Check any custom controls */
for(i = V4L2_CID_PRIVATE_BASE; ; i++) {
ctrl.id = i;
if(IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl) == 0) {
control_readed(vd, &ctrl, pglobal, id);
} else {
break;
}
}
}
memset(&pglobal->in[id].jpegcomp, 0, sizeof(struct v4l2_jpegcompression));
if(xioctl(vd->fd, VIDIOC_G_JPEGCOMP, &pglobal->in[id].jpegcomp) != EINVAL) {
DBG("JPEG compression details:\n");
DBG("Quality: %d\n", pglobal->in[id].jpegcomp.quality);
DBG("APPn: %d\n", pglobal->in[id].jpegcomp.APPn);
DBG("APP length: %d\n", pglobal->in[id].jpegcomp.APP_len);
DBG("APP data: %s\n", pglobal->in[id].jpegcomp.APP_data);
DBG("COM length: %d\n", pglobal->in[id].jpegcomp.COM_len);
DBG("COM data: %s\n", pglobal->in[id].jpegcomp.COM_data);
struct v4l2_queryctrl ctrl_jpeg;
ctrl_jpeg.id = 1;
sprintf((char*)&ctrl_jpeg.name, "JPEG quality");
ctrl_jpeg.minimum = 0;
ctrl_jpeg.maximum = 100;
ctrl_jpeg.step = 1;
ctrl_jpeg.default_value = 50;
ctrl_jpeg.flags = 0;
ctrl_jpeg.type = V4L2_CTRL_TYPE_INTEGER;
if (pglobal->in[id].in_parameters == NULL) {
pglobal->in[id].in_parameters = (control*)calloc(1, sizeof(control));
} else {
pglobal->in[id].in_parameters = (control*)realloc(pglobal->in[id].in_parameters,(pglobal->in[id].parametercount + 1) * sizeof(control));
}
if (pglobal->in[id].in_parameters == NULL) {
DBG("Calloc/realloc failed\n");
return;
}
memcpy(&pglobal->in[id].in_parameters[pglobal->in[id].parametercount].ctrl, &ctrl_jpeg, sizeof(struct v4l2_queryctrl));
pglobal->in[id].in_parameters[pglobal->in[id].parametercount].group = IN_CMD_JPEG_QUALITY;
pglobal->in[id].in_parameters[pglobal->in[id].parametercount].value = pglobal->in[id].jpegcomp.quality;
pglobal->in[id].parametercount++;
} else {
DBG("Modifying the setting of the JPEG compression is not supported\n");
pglobal->in[id].jpegcomp.quality = -1;
}
}
int init_videoIn(struct vdIn *vd, char *device, int width,
int height, int fps, int format, int grabmethod, globals *pglobal)
{
if (vd == NULL || device == NULL)
return -1;
if (width == 0 || height == 0)
return -1;
if (grabmethod < 0 || grabmethod > 1)
grabmethod = 1; //mmap by default;
vd->videodevice = NULL;
vd->status = NULL;
vd->pictName = NULL;
vd->videodevice = (char *) calloc (1, 16 * sizeof (char));
vd->status = (char *) calloc (1, 100 * sizeof (char));
vd->pictName = (char *) calloc (1, 80 * sizeof (char));
snprintf (vd->videodevice, 16-1, "%s", device);
vd->toggleAvi = 0;
vd->getPict = 0;
vd->signalquit = 1;
vd->width = width;
vd->height = height;
vd->fps = fps;
vd->formatIn = format;
vd->grabmethod = grabmethod;
if (init_v4l2 (vd) < 0) {
fprintf (stderr, " Init v4L2 failed !! exit fatal \n");
goto error;;
}
// enumerating v4l2 controls
struct v4l2_queryctrl ctrl;
pglobal->in.parametercount = 0;
pglobal->in.in_parameters = malloc(0 * sizeof(input_control));
/* Enumerate the v4l2 controls
Try the extended control API first */
#ifdef V4L2_CTRL_FLAG_NEXT_CTRL
ctrl.id = V4L2_CTRL_FLAG_NEXT_CTRL;
if(0 == IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl)) {
do {
control_readed(vd, &ctrl, pglobal);
ctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
} while(0 == ioctl (vd->fd, VIDIOC_QUERYCTRL, &ctrl));
} else
#endif
{
/* Fall back on the standard API */
/* Check all the standard controls */
int i;
for(i = V4L2_CID_BASE; i<V4L2_CID_LASTP1; i++) {
ctrl.id = i;
if(IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl) == 0) {
control_readed(vd, &ctrl, pglobal);
}
}
/* Check any custom controls */
for(i=V4L2_CID_PRIVATE_BASE; ; i++) {
ctrl.id = i;
if(IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCTRL, &ctrl) == 0) {
control_readed(vd, &ctrl, pglobal);
} else {
break;
}
}
}
// enumerating formats
int currentWidth, currentHeight = 0;
struct v4l2_format currentFormat;
currentFormat.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (IOCTL_VIDEO(vd->fd, VIDIOC_G_FMT, ¤tFormat) == 0) {
currentWidth = currentFormat.fmt.pix.width;
currentHeight = currentFormat.fmt.pix.height;
DBG("%Current size: dx%d\n", currentWidth, currentHeight);
}
pglobal->in.in_formats = malloc(0 * sizeof(struct v4l2_fmtdesc));
for (pglobal->in.formatCount = 0; 1; pglobal->in.formatCount++) {
struct v4l2_fmtdesc fmtdesc;
fmtdesc.index = pglobal->in.formatCount;
fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (IOCTL_VIDEO(vd->fd, VIDIOC_ENUM_FMT, &fmtdesc) < 0) {
break;
}
pglobal->in.in_formats =
(input_format*)realloc(pglobal->in.in_formats, (pglobal->in.formatCount + 1) * sizeof(input_format));
memcpy(&pglobal->in.in_formats[pglobal->in.formatCount], &fmtdesc, sizeof(input_format));
if (fmtdesc.pixelformat == format)
pglobal->in.currentFormat = pglobal->in.formatCount;
DBG("Supported format: %s\n", fmtdesc.description);
struct v4l2_frmsizeenum fsenum;
fsenum.index = pglobal->in.formatCount;
fsenum.pixel_format = fmtdesc.pixelformat;
int j = 0;
pglobal->in.in_formats[pglobal->in.formatCount].supportedResolutions = malloc(0);
pglobal->in.in_formats[pglobal->in.formatCount].resolutionCount = 0;
pglobal->in.in_formats[pglobal->in.formatCount].currentResolution = -1;
while (1) {
fsenum.index = j;
j++;
if(IOCTL_VIDEO(vd->fd, VIDIOC_ENUM_FRAMESIZES, &fsenum) == 0) {
pglobal->in.in_formats[pglobal->in.formatCount].resolutionCount++;
pglobal->in.in_formats[pglobal->in.formatCount].supportedResolutions = (input_resolution*)
realloc(pglobal->in.in_formats[pglobal->in.formatCount].supportedResolutions, j * sizeof(input_resolution));
pglobal->in.in_formats[pglobal->in.formatCount].supportedResolutions[j-1].width = fsenum.discrete.width;
pglobal->in.in_formats[pglobal->in.formatCount].supportedResolutions[j-1].height = fsenum.discrete.height;
if (format == fmtdesc.pixelformat) {
pglobal->in.in_formats[pglobal->in.formatCount].currentResolution = (j-1);
DBG("\tSupported size with the current format: %dx%d\n", fsenum.discrete.width, fsenum.discrete.height);
} else {
DBG("\tSupported size: %dx%d\n", fsenum.discrete.width, fsenum.discrete.height);
}
} else {
break;
}
}
}
/* alloc a temp buffer to reconstruct the pict */
vd->framesizeIn = (vd->width * vd->height << 1);
switch (vd->formatIn) {
case V4L2_PIX_FMT_MJPEG:
vd->tmpbuffer = (unsigned char *) calloc(1, (size_t) vd->framesizeIn);
if (!vd->tmpbuffer)
goto error;
vd->framebuffer =
(unsigned char *) calloc(1, (size_t) vd->width * (vd->height + 8) * 2);
break;
case V4L2_PIX_FMT_YUYV:
vd->framebuffer =
(unsigned char *) calloc(1, (size_t) vd->framesizeIn);
break;
default:
fprintf(stderr, " should never arrive exit fatal !!\n");
goto error;
break;
}
if (!vd->framebuffer)
goto error;
return 0;
error:
free(pglobal->in.in_parameters);
free(vd->videodevice);
free(vd->status);
free(vd->pictName);
close(vd->fd);
return -1;
}
int uvcGrab(struct vdIn *vd)
{
#define HEADERFRAME1 0xaf
int ret;
if (vd->streamingState == STREAMING_OFF) {
if (video_enable(vd))
goto err;
}
memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd->buf.memory = V4L2_MEMORY_MMAP;
ret = IOCTL_VIDEO(vd->fd, VIDIOC_DQBUF, &vd->buf);
if (ret < 0) {
perror("Unable to dequeue buffer");
goto err;
}
switch (vd->formatIn) {
case V4L2_PIX_FMT_MJPEG:
if (vd->buf.bytesused <= HEADERFRAME1) { /* Prevent crash
* on empty image */
fprintf(stderr, "Ignoring empty buffer ...\n");
return 0;
}
/* memcpy(vd->tmpbuffer, vd->mem[vd->buf.index], vd->buf.bytesused);
memcpy (vd->tmpbuffer, vd->mem[vd->buf.index], HEADERFRAME1);
memcpy (vd->tmpbuffer + HEADERFRAME1, dht_data, sizeof(dht_data));
memcpy (vd->tmpbuffer + HEADERFRAME1 + sizeof(dht_data), vd->mem[vd->buf.index] + HEADERFRAME1, (vd->buf.bytesused - HEADERFRAME1));
*/
memcpy(vd->tmpbuffer, vd->mem[vd->buf.index], vd->buf.bytesused);
if (debug)
fprintf(stderr, "bytes in used %d \n", vd->buf.bytesused);
break;
case V4L2_PIX_FMT_YUYV:
if (vd->buf.bytesused > vd->framesizeIn)
memcpy (vd->framebuffer, vd->mem[vd->buf.index], (size_t) vd->framesizeIn);
else
memcpy (vd->framebuffer, vd->mem[vd->buf.index], (size_t) vd->buf.bytesused);
break;
default:
goto err;
break;
}
ret = IOCTL_VIDEO(vd->fd, VIDIOC_QBUF, &vd->buf);
if (ret < 0) {
perror("Unable to requeue buffer");
goto err;
}
return 0;
err:
vd->signalquit = 0;
return -1;
}
static int init_v4l2(struct vdIn *vd)
{
int i;
int ret = 0;
if ((vd->fd = OPEN_VIDEO(vd->videodevice, O_RDWR)) == -1) {
perror("ERROR opening V4L interface");
return -1;
}
memset(&vd->cap, 0, sizeof(struct v4l2_capability));
ret = IOCTL_VIDEO(vd->fd, VIDIOC_QUERYCAP, &vd->cap);
if (ret < 0) {
fprintf(stderr, "Error opening device %s: unable to query device.\n", vd->videodevice);
goto fatal;
}
if ((vd->cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0) {
fprintf(stderr, "Error opening device %s: video capture not supported.\n",
vd->videodevice);
goto fatal;;
}
if (vd->grabmethod) {
if (!(vd->cap.capabilities & V4L2_CAP_STREAMING)) {
fprintf(stderr, "%s does not support streaming i/o\n", vd->videodevice);
goto fatal;
}
} else {
if (!(vd->cap.capabilities & V4L2_CAP_READWRITE)) {
fprintf(stderr, "%s does not support read i/o\n", vd->videodevice);
goto fatal;
}
}
/*
* set format in
*/
memset(&vd->fmt, 0, sizeof(struct v4l2_format));
vd->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd->fmt.fmt.pix.width = vd->width;
vd->fmt.fmt.pix.height = vd->height;
vd->fmt.fmt.pix.pixelformat = vd->formatIn;
vd->fmt.fmt.pix.field = V4L2_FIELD_ANY;
ret = IOCTL_VIDEO(vd->fd, VIDIOC_S_FMT, &vd->fmt);
if (ret < 0) {
fprintf(stderr, "Unable to set format: %d res: %dx%d", vd->formatIn, vd->width, vd->height);
goto fatal;
}
if ((vd->fmt.fmt.pix.width != vd->width) ||
(vd->fmt.fmt.pix.height != vd->height)) {
fprintf(stderr, "i: The format asked unavailable, so the width %d height %d \n", vd->fmt.fmt.pix.width, vd->fmt.fmt.pix.height);
vd->width = vd->fmt.fmt.pix.width;
vd->height = vd->fmt.fmt.pix.height;
/*
* look the format is not part of the deal ???
*/
if (vd->formatIn != vd->fmt.fmt.pix.pixelformat) {
if (vd->formatIn == V4L2_PIX_FMT_MJPEG) {
fprintf(stderr, "The inpout device does not supports MJPEG mode\nYou may also try the YUV mode (-yuv option), but it requires a much more CPU power\n");
goto fatal;
} else if (vd->formatIn == V4L2_PIX_FMT_YUYV) {
fprintf(stderr, "The input device does not supports YUV mode\n");
goto fatal;
}
} else {
vd->formatIn = vd->fmt.fmt.pix.pixelformat;
}
}
/*
* set framerate
*/
struct v4l2_streamparm *setfps;
setfps = (struct v4l2_streamparm *) calloc(1, sizeof(struct v4l2_streamparm));
if (setfps == NULL) {
perror("Unable to allocate param buffer");
goto fatal;
}
memset(setfps, 0, sizeof(struct v4l2_streamparm));
setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
setfps->parm.capture.timeperframe.numerator = 1;
setfps->parm.capture.timeperframe.denominator = vd->fps;
ret = IOCTL_VIDEO(vd->fd, VIDIOC_S_PARM, setfps);
free(setfps);
/*
* request buffers
*/
memset(&vd->rb, 0, sizeof(struct v4l2_requestbuffers));
vd->rb.count = NB_BUFFER;
vd->rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd->rb.memory = V4L2_MEMORY_MMAP;
ret = IOCTL_VIDEO(vd->fd, VIDIOC_REQBUFS, &vd->rb);
if (ret < 0) {
perror("Unable to allocate buffers");
goto fatal;
}
/*
* map the buffers
*/
for (i = 0; i < NB_BUFFER; i++) {
memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
vd->buf.index = i;
vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd->buf.memory = V4L2_MEMORY_MMAP;
ret = IOCTL_VIDEO(vd->fd, VIDIOC_QUERYBUF, &vd->buf);
if (ret < 0) {
perror("Unable to query buffer");
goto fatal;
}
if (debug)
fprintf(stderr, "length: %u offset: %u\n", vd->buf.length, vd->buf.m.offset);
vd->mem[i] = mmap(0 /* start anywhere */ ,
vd->buf.length, PROT_READ, MAP_SHARED, vd->fd,
vd->buf.m.offset);
if (vd->mem[i] == MAP_FAILED) {
perror("Unable to map buffer");
goto fatal;
}
if (debug)
fprintf(stderr, "Buffer mapped at address %p.\n", vd->mem[i]);
}
/*
* Queue the buffers.
*/
for (i = 0; i < NB_BUFFER; ++i) {
memset(&vd->buf, 0, sizeof(struct v4l2_buffer));
vd->buf.index = i;
vd->buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
vd->buf.memory = V4L2_MEMORY_MMAP;
ret = IOCTL_VIDEO(vd->fd, VIDIOC_QBUF, &vd->buf);
if (ret < 0) {
perror("Unable to queue buffer");
goto fatal;;
}
}
return 0;
fatal:
return -1;
}