tizen/src/hw/svcamera_linux.c

   1 /*
   2  * Virtual Camera device(PCI) for Linux host.
   3  *
   4  * Copyright (c) 2011 Samsung Electronics Co., Ltd All Rights Reserved
   5  *
   6  * Contact:
   7  * JinHyung Jo <jinhyung.jo@samsung.com>
   8  * YeongKyoon Lee <yeongkyoon.lee@samsung.com>
   9  * DongKyun Yun <dk77.yun@samsung.com>
  10  *
  11  * This program is free software; you can redistribute it and/or
  12  * modify it under the terms of the GNU General Public License
  13  * as published by the Free Software Foundation; either version 2
  14  * of the License, or (at your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful,
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19  * GNU General Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program; if not, write to the Free Software
  23  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  24  *
  25  * Contributors:
  26  * - S-Core Co., Ltd
  27  *
  28  */
  29
  30 #include "qemu-common.h"
  31 #include "qemu-common.h"
  32 #include "svcamera.h"
  33 #include "pci.h"
  34 #include "kvm.h"
  35 #include "tizen/src/debug_ch.h"
  36
  37 #include <linux/videodev2.h>
  38
  39 #include <libv4l2.h>
  40 #include <libv4lconvert.h>
  41
  42 MULTI_DEBUG_CHANNEL(tizen, camera_linux);
  43
  44 static int v4l2_fd;
  45 static int convert_trial;
  46
  47 static struct v4l2_format dst_fmt;
  48
  49 static int xioctl(int fd, int req, void *arg)
  50 {
  51     int r;
  52
  53     do {
  54         r = v4l2_ioctl(fd, req, arg);
  55     } while ( r < 0 && errno == EINTR);
  56
  57     return r;
  58 }
  59
  60 #define SVCAM_CTRL_VALUE_MAX        20
  61 #define SVCAM_CTRL_VALUE_MIN        1
  62 #define SVCAM_CTRL_VALUE_MID        10
  63 #define SVCAM_CTRL_VALUE_STEP       1
  64
  65 struct svcam_qctrl {
  66     uint32_t id;
  67     uint32_t hit;
  68     int32_t min;
  69     int32_t max;
  70     int32_t step;
  71     int32_t init_val;
  72 };
  73
  74 static struct svcam_qctrl qctrl_tbl[] = {
  75     { V4L2_CID_BRIGHTNESS, 0, },
  76     { V4L2_CID_CONTRAST, 0, },
  77     { V4L2_CID_SATURATION,0, },
  78     { V4L2_CID_SHARPNESS, 0, },
  79 };
  80
  81 static void svcam_reset_controls(void)
  82 {
  83     uint32_t i;
  84     for (i = 0; i < ARRAY_SIZE(qctrl_tbl); i++) {
  85         if (qctrl_tbl[i].hit) {
  86             struct v4l2_control ctrl = {0,};
  87             ctrl.id = qctrl_tbl[i].id;
  88             ctrl.value = qctrl_tbl[i].init_val;
  89             if (xioctl(v4l2_fd, VIDIOC_S_CTRL, &ctrl) < 0) {
  90                 ERR("failed to set video control value while reset values : %s\n", strerror(errno));
  91             }
  92         }
  93     }
  94 }
  95
  96 static int32_t value_convert_from_guest(int32_t min, int32_t max, int32_t value)
  97 {
  98     double rate = 0.0;
  99     int32_t dist = 0, ret = 0;
 100
 101     dist = max - min;
 102
 103     if (dist < SVCAM_CTRL_VALUE_MAX) {
 104         rate = (double)SVCAM_CTRL_VALUE_MAX / (double)dist;
 105         ret = min + (int32_t)(value / rate);
 106     } else {
 107         rate = (double)dist / (double)SVCAM_CTRL_VALUE_MAX;
 108         ret = min + (int32_t)(rate * value);
 109     }
 110     return ret;
 111 }
 112
 113 static int32_t value_convert_to_guest(int32_t min, int32_t max, int32_t value)
 114 {
 115     double rate  = 0.0;
 116     int32_t dist = 0, ret = 0;
 117
 118     dist = max - min;
 119
 120     if (dist < SVCAM_CTRL_VALUE_MAX) {
 121         rate = (double)SVCAM_CTRL_VALUE_MAX / (double)dist;
 122         ret = (int32_t)((double)(value - min) * rate);
 123     } else {
 124         rate = (double)dist / (double)SVCAM_CTRL_VALUE_MAX;
 125         ret = (int32_t)((double)(value - min) / rate);
 126     }
 127
 128     return ret;
 129 }
 130
 131 static int __v4l2_grab(SVCamState *state)
 132 {
 133     fd_set fds;
 134     static uint32_t index = 0;
 135     struct timeval tv;
 136     void *buf;
 137     int ret;
 138
 139     FD_ZERO(&fds);
 140     FD_SET(v4l2_fd, &fds);
 141
 142     tv.tv_sec = 2;
 143     tv.tv_usec = 0;
 144
 145     ret = select(v4l2_fd + 1, &fds, NULL, NULL, &tv);
 146     if ( ret < 0) {
 147         if (errno == EINTR)
 148             return 0;
 149         ERR("select : %s\n", strerror(errno));
 150         return -1;
 151     }
 152     if (!ret) {
 153         WARN("Timed out\n");
 154         return 0;
 155     }
 156
 157     if (!v4l2_fd) {
 158         WARN("file descriptor is closed or not opened \n");
 159         return -1;
 160     }
 161
 162     buf = state->vaddr + (state->buf_size * index);
 163     ret = v4l2_read(v4l2_fd, buf, state->buf_size);
 164     if ( ret < 0) {
 165         switch (errno) {
 166         case EINVAL:
 167         case ENOMEM:
 168             ERR("v4l2_read failed : %s\n", strerror(errno));
 169             return -1;
 170         case EAGAIN:
 171         case EIO:
 172         case EINTR:
 173         default:
 174             if (convert_trial-- == -1) {
 175                 ERR("Try count for v4l2_read is exceeded\n");
 176                 return -1;
 177             }
 178             return 0;
 179         }
 180     }
 181
 182     index = !index;
 183
 184     pthread_mutex_lock(&state->thread->mutex_lock);
 185     if (state->streamon) {
 186         if (state->req_frame) {
 187             qemu_irq_raise(state->dev.irq[2]);
 188             state->req_frame = 0;
 189         }
 190         ret = 1;
 191     } else {
 192         ret = -1;
 193     }
 194     pthread_mutex_unlock(&state->thread->mutex_lock);
 195
 196     return ret;
 197 }
 198
 199 // Worker thread
 200 static void *svcam_worker_thread(void *thread_param)
 201 {
 202     SVCamThreadInfo* thread = (SVCamThreadInfo*)thread_param;
 203
 204 wait_worker_thread:
 205     pthread_mutex_lock(&thread->mutex_lock);
 206     thread->state->streamon = 0;
 207     convert_trial = 10;
 208     pthread_cond_wait(&thread->thread_cond, &thread->mutex_lock);
 209     pthread_mutex_unlock(&thread->mutex_lock);
 210     INFO("Streaming on ......\n");
 211
 212     while (1)
 213     {
 214         pthread_mutex_lock(&thread->mutex_lock);
 215         if (thread->state->streamon) {
 216             pthread_mutex_unlock(&thread->mutex_lock);
 217             if (__v4l2_grab(thread->state) < 0) {
 218                 INFO("...... Streaming off\n");
 219                 goto wait_worker_thread;
 220             }
 221         } else {
 222             pthread_mutex_unlock(&thread->mutex_lock);
 223             goto wait_worker_thread;
 224         }
 225     }
 226     pthread_exit(0);
 227 }
 228
 229 void svcam_device_init(SVCamState* state)
 230 {
 231     SVCamThreadInfo *thread = state->thread;
 232
 233     if (pthread_cond_init(&thread->thread_cond, NULL)) {
 234         ERR("failed to initialize thread condition\n");
 235         exit(61);
 236     }
 237     if (pthread_mutex_init(&thread->mutex_lock, NULL)) {
 238         ERR("failed to initialize mutex\n");
 239         exit(62);
 240     }
 241
 242     if (pthread_create(&thread->thread_id, NULL, svcam_worker_thread, (void*)thread) != 0) {
 243         perror("failed to create a worker thread for webcam connection\n");
 244         exit(63);
 245     }
 246 }
 247
 248 // SVCAM_CMD_OPEN
 249 void svcam_device_open(SVCamState* state)
 250 {
 251     struct v4l2_capability cap;
 252     SVCamParam *param = state->thread->param;
 253
 254     param->top = 0;
 255     v4l2_fd = v4l2_open("/dev/video0", O_RDWR | O_NONBLOCK);
 256     if (v4l2_fd < 0) {
 257         ERR("v4l2 device open failed.(/dev/video0)\n");
 258         param->errCode = EINVAL;
 259         return;
 260     }
 261     if (xioctl(v4l2_fd, VIDIOC_QUERYCAP, &cap) < 0) {
 262         ERR("Could not qeury video capabilities\n");
 263         v4l2_close(v4l2_fd);
 264         param->errCode = EINVAL;
 265         return;
 266     }
 267     if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) ||
 268             !(cap.capabilities & V4L2_CAP_STREAMING)) {
 269         ERR("Not supported video driver.\n");
 270         v4l2_close(v4l2_fd);
 271         param->errCode = EINVAL;
 272         return;
 273     }
 274
 275     memset(&dst_fmt, 0, sizeof(dst_fmt));
 276     INFO("Opened\n");
 277 }
 278
 279 // SVCAM_CMD_START_PREVIEW
 280 void svcam_device_start_preview(SVCamState* state)
 281 {
 282     pthread_mutex_lock(&state->thread->mutex_lock);
 283     state->streamon = 1;
 284     state->buf_size = dst_fmt.fmt.pix.sizeimage;
 285     if (pthread_cond_signal(&state->thread->thread_cond))
 286         ERR("failed to send a signal to the worker thread\n");
 287     pthread_mutex_unlock(&state->thread->mutex_lock);
 288 }
 289
 290 // SVCAM_CMD_STOP_PREVIEW
 291 void svcam_device_stop_preview(SVCamState* state)
 292 {
 293     pthread_mutex_lock(&state->thread->mutex_lock);
 294     state->streamon = 0;
 295     state->buf_size = 0;
 296     pthread_mutex_unlock(&state->thread->mutex_lock);
 297     sleep(0);
 298 }
 299
 300 void svcam_device_s_param(SVCamState* state)
 301 {
 302     struct v4l2_streamparm sp;
 303     SVCamParam *param = state->thread->param;
 304
 305     param->top = 0;
 306     memset(&sp, 0, sizeof(struct v4l2_streamparm));
 307     sp.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 308     sp.parm.capture.timeperframe.numerator = param->stack[0];
 309     sp.parm.capture.timeperframe.denominator = param->stack[1];
 310
 311     if (xioctl(v4l2_fd, VIDIOC_S_PARM, &sp) < 0) {
 312         ERR("failed to set FPS: %s\n", strerror(errno));
 313         param->errCode = errno;
 314     }
 315 }
 316
 317 void svcam_device_g_param(SVCamState* state)
 318 {
 319     struct v4l2_streamparm sp;
 320     SVCamParam *param = state->thread->param;
 321
 322     param->top = 0;
 323     memset(&sp, 0, sizeof(struct v4l2_streamparm));
 324     sp.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 325
 326     if (xioctl(v4l2_fd, VIDIOC_G_PARM, &sp) < 0) {
 327         ERR("failed to get FPS: %s\n", strerror(errno));
 328         param->errCode = errno;
 329         return;
 330     }
 331     param->stack[0] = sp.parm.capture.capability;
 332     param->stack[1] = sp.parm.capture.timeperframe.numerator;
 333     param->stack[2] = sp.parm.capture.timeperframe.denominator;
 334 }
 335
 336 void svcam_device_s_fmt(SVCamState* state)
 337 {
 338     SVCamParam *param = state->thread->param;
 339
 340     param->top = 0;
 341     memset(&dst_fmt, 0, sizeof(struct v4l2_format));
 342     dst_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 343     dst_fmt.fmt.pix.width = param->stack[0];
 344     dst_fmt.fmt.pix.height = param->stack[1];
 345     dst_fmt.fmt.pix.pixelformat = param->stack[2];
 346     dst_fmt.fmt.pix.field = param->stack[3];
 347
 348     if (xioctl(v4l2_fd, VIDIOC_S_FMT, &dst_fmt) < 0) {
 349         ERR("failed to set video format: %s\n", strerror(errno));
 350         param->errCode = errno;
 351         return;
 352     }
 353
 354     param->stack[0] = dst_fmt.fmt.pix.width;
 355     param->stack[1] = dst_fmt.fmt.pix.height;
 356     param->stack[2] = dst_fmt.fmt.pix.field;
 357     param->stack[3] = dst_fmt.fmt.pix.pixelformat;
 358     param->stack[4] = dst_fmt.fmt.pix.bytesperline;
 359     param->stack[5] = dst_fmt.fmt.pix.sizeimage;
 360     param->stack[6] = dst_fmt.fmt.pix.colorspace;
 361     param->stack[7] = dst_fmt.fmt.pix.priv;
 362 }
 363
 364 void svcam_device_g_fmt(SVCamState* state)
 365 {
 366     struct v4l2_format format;
 367     SVCamParam *param = state->thread->param;
 368
 369     param->top = 0;
 370     memset(&format, 0, sizeof(struct v4l2_format));
 371     format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 372
 373     if (xioctl(v4l2_fd, VIDIOC_G_FMT, &format) < 0) {
 374         ERR("failed to get video format: %s\n", strerror(errno));
 375         param->errCode = errno;
 376     } else {
 377         param->stack[0] = format.fmt.pix.width;
 378         param->stack[1] = format.fmt.pix.height;
 379         param->stack[2] = format.fmt.pix.field;
 380         param->stack[3] = format.fmt.pix.pixelformat;
 381         param->stack[4] = format.fmt.pix.bytesperline;
 382         param->stack[5] = format.fmt.pix.sizeimage;
 383         param->stack[6] = format.fmt.pix.colorspace;
 384         param->stack[7] = format.fmt.pix.priv;
 385         memcpy(&dst_fmt, &format, sizeof(format));
 386     }
 387 }
 388
 389 void svcam_device_try_fmt(SVCamState* state)
 390 {
 391     struct v4l2_format format;
 392     SVCamParam *param = state->thread->param;
 393
 394     param->top = 0;
 395     memset(&format, 0, sizeof(struct v4l2_format));
 396     format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 397     format.fmt.pix.width = param->stack[0];
 398     format.fmt.pix.height = param->stack[1];
 399     format.fmt.pix.pixelformat = param->stack[2];
 400     format.fmt.pix.field = param->stack[3];
 401
 402     if (xioctl(v4l2_fd, VIDIOC_TRY_FMT, &format) < 0) {
 403         ERR("failed to check video format: %s\n", strerror(errno));
 404         param->errCode = errno;
 405         return;
 406     }
 407     param->stack[0] = format.fmt.pix.width;
 408     param->stack[1] = format.fmt.pix.height;
 409     param->stack[2] = format.fmt.pix.field;
 410     param->stack[3] = format.fmt.pix.pixelformat;
 411     param->stack[4] = format.fmt.pix.bytesperline;
 412     param->stack[5] = format.fmt.pix.sizeimage;
 413     param->stack[6] = format.fmt.pix.colorspace;
 414     param->stack[7] = format.fmt.pix.priv;
 415 }
 416
 417 void svcam_device_enum_fmt(SVCamState* state)
 418 {
 419     struct v4l2_fmtdesc format;
 420     SVCamParam *param = state->thread->param;
 421
 422     param->top = 0;
 423     memset(&format, 0, sizeof(struct v4l2_fmtdesc));
 424     format.index = param->stack[0];
 425     format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 426
 427     if (xioctl(v4l2_fd, VIDIOC_ENUM_FMT, &format) < 0) {
 428         if (errno != EINVAL)
 429             ERR("failed to enumerate video formats: %s\n", strerror(errno));
 430         param->errCode = errno;
 431         return;
 432     }
 433     param->stack[0] = format.index;
 434     param->stack[1] = format.flags;
 435     param->stack[2] = format.pixelformat;
 436     /* set description */
 437     memcpy(&param->stack[3], format.description, sizeof(format.description));
 438 }
 439
 440 void svcam_device_qctrl(SVCamState* state)
 441 {
 442     uint32_t i;
 443     char name[32] = {0,};
 444     struct v4l2_queryctrl ctrl;
 445     SVCamParam *param = state->thread->param;
 446
 447     param->top = 0;
 448     memset(&ctrl, 0, sizeof(struct v4l2_queryctrl));
 449     ctrl.id = param->stack[0];
 450
 451     switch (ctrl.id) {
 452     case V4L2_CID_BRIGHTNESS:
 453         TRACE("Query : BRIGHTNESS\n");
 454         memcpy((void*)name, (void*)"brightness", 32);
 455         i = 0;
 456         break;
 457     case V4L2_CID_CONTRAST:
 458         TRACE("Query : CONTRAST\n");
 459         memcpy((void*)name, (void*)"contrast", 32);
 460         i = 1;
 461         break;
 462     case V4L2_CID_SATURATION:
 463         TRACE("Query : SATURATION\n");
 464         memcpy((void*)name, (void*)"saturation", 32);
 465         i = 2;
 466         break;
 467     case V4L2_CID_SHARPNESS:
 468         TRACE("Query : SHARPNESS\n");
 469         memcpy((void*)name, (void*)"sharpness", 32);
 470         i = 3;
 471         break;
 472     default:
 473         param->errCode = EINVAL;
 474         return;
 475     }
 476
 477     if (xioctl(v4l2_fd, VIDIOC_QUERYCTRL, &ctrl) < 0) {
 478         if (errno != EINVAL)
 479             ERR("failed to query video controls : %s\n", strerror(errno));
 480         param->errCode = errno;
 481         return;
 482     } else {
 483         struct v4l2_control sctrl;
 484         memset(&sctrl, 0, sizeof(struct v4l2_control));
 485         sctrl.id = ctrl.id;
 486         if ((ctrl.maximum + ctrl.minimum) == 0) {
 487             sctrl.value = 0;
 488         } else {
 489             sctrl.value = (ctrl.maximum + ctrl.minimum) / 2;
 490         }
 491         if (xioctl(v4l2_fd, VIDIOC_S_CTRL, &sctrl) < 0) {
 492             ERR("failed to set video control value : %s\n", strerror(errno));
 493             param->errCode = errno;
 494             return;
 495         }
 496         qctrl_tbl[i].hit = 1;
 497         qctrl_tbl[i].min = ctrl.minimum;
 498         qctrl_tbl[i].max = ctrl.maximum;
 499         qctrl_tbl[i].step = ctrl.step;
 500         qctrl_tbl[i].init_val = ctrl.default_value;
 501     }
 502
 503     // set fixed values by FW configuration file
 504     param->stack[0] = ctrl.id;
 505     param->stack[1] = SVCAM_CTRL_VALUE_MIN; // minimum
 506     param->stack[2] = SVCAM_CTRL_VALUE_MAX; // maximum
 507     param->stack[3] = SVCAM_CTRL_VALUE_STEP;// step
 508     param->stack[4] = SVCAM_CTRL_VALUE_MID; // default_value
 509     param->stack[5] = ctrl.flags;
 510     /* name field setting */
 511     memcpy(&param->stack[6], (void*)name, sizeof(ctrl.name));
 512 }
 513
 514 void svcam_device_s_ctrl(SVCamState* state)
 515 {
 516     uint32_t i;
 517     struct v4l2_control ctrl;
 518     SVCamParam *param = state->thread->param;
 519
 520     param->top = 0;
 521     memset(&ctrl, 0, sizeof(struct v4l2_control));
 522     ctrl.id = param->stack[0];
 523
 524     switch (ctrl.id) {
 525     case V4L2_CID_BRIGHTNESS:
 526         i = 0;
 527         TRACE("%d is set to the value of the BRIGHTNESS\n", param->stack[1]);
 528         break;
 529     case V4L2_CID_CONTRAST:
 530         i = 1;
 531         TRACE("%d is set to the value of the CONTRAST\n", param->stack[1]);
 532         break;
 533     case V4L2_CID_SATURATION:
 534         i = 2;
 535         TRACE("%d is set to the value of the SATURATION\n", param->stack[1]);
 536         break;
 537     case V4L2_CID_SHARPNESS:
 538         i = 3;
 539         TRACE("%d is set to the value of the SHARPNESS\n", param->stack[1]);
 540         break;
 541     default:
 542         ERR("our emulator does not support this control : 0x%x\n", ctrl.id);
 543         param->errCode = EINVAL;
 544         return;
 545     }
 546
 547     ctrl.value = value_convert_from_guest(qctrl_tbl[i].min,
 548             qctrl_tbl[i].max, param->stack[1]);
 549     if (xioctl(v4l2_fd, VIDIOC_S_CTRL, &ctrl) < 0) {
 550         ERR("failed to set video control value : value(%d), %s\n", param->stack[1], strerror(errno));
 551         param->errCode = errno;
 552         return;
 553     }
 554 }
 555
 556 void svcam_device_g_ctrl(SVCamState* state)
 557 {
 558     uint32_t i;
 559     struct v4l2_control ctrl;
 560     SVCamParam *param = state->thread->param;
 561
 562     param->top = 0;
 563     memset(&ctrl, 0, sizeof(struct v4l2_control));
 564     ctrl.id = param->stack[0];
 565
 566     switch (ctrl.id) {
 567     case V4L2_CID_BRIGHTNESS:
 568         TRACE("Gets the value of the BRIGHTNESS\n");
 569         i = 0;
 570         break;
 571     case V4L2_CID_CONTRAST:
 572         TRACE("Gets the value of the CONTRAST\n");
 573         i = 1;
 574         break;
 575     case V4L2_CID_SATURATION:
 576         TRACE("Gets the value of the SATURATION\n");
 577         i = 2;
 578         break;
 579     case V4L2_CID_SHARPNESS:
 580         TRACE("Gets the value of the SHARPNESS\n");
 581         i = 3;
 582         break;
 583     default:
 584         ERR("our emulator does not support this control : 0x%x\n", ctrl.id);
 585         param->errCode = EINVAL;
 586         return;
 587     }
 588
 589     if (xioctl(v4l2_fd, VIDIOC_G_CTRL, &ctrl) < 0) {
 590         ERR("failed to get video control value : %s\n", strerror(errno));
 591         param->errCode = errno;
 592         return;
 593     }
 594     param->stack[0] = value_convert_to_guest(qctrl_tbl[i].min,
 595             qctrl_tbl[i].max, ctrl.value);
 596     TRACE("Value : %d\n", param->stack[0]);
 597 }
 598
 599 void svcam_device_enum_fsizes(SVCamState* state)
 600 {
 601     struct v4l2_frmsizeenum fsize;
 602     SVCamParam *param = state->thread->param;
 603
 604     param->top = 0;
 605     memset(&fsize, 0, sizeof(struct v4l2_frmsizeenum));
 606     fsize.index = param->stack[0];
 607     fsize.pixel_format = param->stack[1];
 608
 609     if (xioctl(v4l2_fd, VIDIOC_ENUM_FRAMESIZES, &fsize) < 0) {
 610         if (errno != EINVAL)
 611             ERR("failed to get frame sizes : %s\n", strerror(errno));
 612         param->errCode = errno;
 613         return;
 614     }
 615
 616     if (fsize.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
 617         param->stack[0] = fsize.discrete.width;
 618         param->stack[1] = fsize.discrete.height;
 619     } else {
 620         param->errCode = EINVAL;
 621         ERR("Not Supported mode, we only support DISCRETE\n");
 622     }
 623 }
 624
 625 void svcam_device_enum_fintv(SVCamState* state)
 626 {
 627     struct v4l2_frmivalenum ival;
 628     SVCamParam *param = state->thread->param;
 629
 630     param->top = 0;
 631     memset(&ival, 0, sizeof(struct v4l2_frmivalenum));
 632     ival.index = param->stack[0];
 633     ival.pixel_format = param->stack[1];
 634     ival.width = param->stack[2];
 635     ival.height = param->stack[3];
 636
 637     if (xioctl(v4l2_fd, VIDIOC_ENUM_FRAMEINTERVALS, &ival) < 0) {
 638         if (errno != EINVAL)
 639             ERR("failed to get frame intervals : %s\n", strerror(errno));
 640         param->errCode = errno;
 641         return;
 642     }
 643
 644     if (ival.type == V4L2_FRMIVAL_TYPE_DISCRETE) {
 645         param->stack[0] = ival.discrete.numerator;
 646         param->stack[1] = ival.discrete.denominator;
 647     } else {
 648         param->errCode = EINVAL;
 649         ERR("Not Supported mode, we only support DISCRETE\n");
 650     }
 651 }
 652
 653 // SVCAM_CMD_CLOSE
 654 void svcam_device_close(SVCamState* state)
 655 {
 656     pthread_mutex_lock(&state->thread->mutex_lock);
 657     state->streamon = 0;
 658     pthread_mutex_unlock(&state->thread->mutex_lock);
 659
 660     svcam_reset_controls();
 661
 662     v4l2_close(v4l2_fd);
 663     v4l2_fd = 0;
 664     INFO("Closed\n");
 665 }