libv4l2.c

trident tm5600的linux驱动

      if (buf.flags & V4L2_BUF_FLAG_MAPPED)
	break;
    }
  } else {
    /* Conversion mode */
    for (i = 0; i < devices[index].no_frames; i++)
      if (devices[index].frame_map_count[i])
	break;
  }

  if (i != devices[index].no_frames)
    V4L2_LOG("v4l2_buffers_mapped(): buffers still mapped\n");

  return i != devices[index].no_frames;
}


int v4l2_open (const char *file, int oflag, ...)
{
  int fd;

  /* original open code */
  if (oflag & O_CREAT)
  {
    va_list ap;
    mode_t mode;

    va_start (ap, oflag);
    mode = va_arg (ap, mode_t);

    fd = syscall(SYS_open, file, oflag, mode);

    va_end(ap);
  }
  else
    fd = syscall(SYS_open, file, oflag);
  /* end of original open code */

  if (fd == -1)
    return fd;

  if (v4l2_fd_open(fd, 0) == -1) {
    int saved_err = errno;
    syscall(SYS_close, fd);
    errno = saved_err;
    return -1;
  }

  return fd;
}

int v4l2_fd_open(int fd, int v4l2_flags)
{
  int i, index;
  char *lfname;
  struct v4l2_capability cap;
  struct v4l2_format fmt;
  struct v4lconvert_data *convert;

  /* If no log file was set by the app, see if one was specified through the
     environment */
  if (!v4l2_log_file && (lfname = getenv("LIBV4L2_LOG_FILENAME")))
    v4l2_log_file = fopen(lfname, "w");

  /* check that this is an v4l2 device */
  if (syscall(SYS_ioctl, fd, VIDIOC_QUERYCAP, &cap)) {
    int saved_err = errno;
    V4L2_LOG_ERR("getting capabilities: %s\n", strerror(errno));
    errno = saved_err;
    return -1;
  }

  /* we only add functionality for video capture devices, and we do not
     handle devices which don't do mmap */
  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) ||
      !(cap.capabilities & V4L2_CAP_STREAMING))
    return fd;

  /* Get current cam format */
  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (syscall(SYS_ioctl, fd, VIDIOC_G_FMT, &fmt)) {
    int saved_err = errno;
    V4L2_LOG_ERR("getting pixformat: %s\n", strerror(errno));
    errno = saved_err;
    return -1;
  }

  /* init libv4lconvert */
  if (!(convert = v4lconvert_create(fd)))
    return -1;

  /* So we have a v4l2 capture device, register it in our devices array */
  pthread_mutex_lock(&v4l2_open_mutex);
  for (index = 0; index < V4L2_MAX_DEVICES; index++)
    if(devices[index].fd == -1) {
      devices[index].fd = fd;
      break;
    }
  pthread_mutex_unlock(&v4l2_open_mutex);

  if (index == V4L2_MAX_DEVICES) {
    V4L2_LOG_ERR("attempting to open more then %d video devices\n",
      V4L2_MAX_DEVICES);
    errno = EBUSY;
    return -1;
  }

  devices[index].flags = v4l2_flags;
  if (cap.capabilities & V4L2_CAP_READWRITE)
    devices[index].flags |= V4L2_SUPPORTS_READ;
  devices[index].open_count = 1;
  devices[index].src_fmt = fmt;
  devices[index].dest_fmt = fmt;

  pthread_mutex_init(&devices[index].stream_lock, NULL);

  devices[index].no_frames = 0;
  devices[index].nreadbuffers = V4L2_DEFAULT_NREADBUFFERS;
  devices[index].convert = convert;
  devices[index].convert_mmap_buf = MAP_FAILED;
  for (i = 0; i < V4L2_MAX_NO_FRAMES; i++) {
    devices[index].frame_pointers[i] = MAP_FAILED;
    devices[index].frame_map_count[i] = 0;
  }
  devices[index].frame_queued = 0;

  if (index >= devices_used)
    devices_used = index + 1;

  V4L2_LOG("open: %d\n", fd);

  return fd;
}

/* Is this an fd for which we are emulating v4l1 ? */
static int v4l2_get_index(int fd)
{
  int index;

  /* We never handle fd -1 */
  if (fd == -1)
    return -1;

  for (index = 0; index < devices_used; index++)
    if (devices[index].fd == fd)
      break;

  if (index == devices_used)
    return -1;

  return index;
}


int v4l2_close(int fd)
{
  int index, result;

  if ((index = v4l2_get_index(fd)) == -1)
    return syscall(SYS_close, fd);

  /* Abuse stream_lock to stop 2 closes from racing and trying to free the
     resources twice */
  pthread_mutex_lock(&devices[index].stream_lock);
  devices[index].open_count--;
  result = devices[index].open_count != 0;
  pthread_mutex_unlock(&devices[index].stream_lock);

  if (result)
    return 0;

  /* Free resources */
  v4l2_unmap_buffers(index);
  v4lconvert_destroy(devices[index].convert);
  if (devices[index].convert_mmap_buf != MAP_FAILED) {
    if (v4l2_buffers_mapped(index))
      V4L2_LOG_WARN("v4l2 mmap buffers still mapped on close()\n");
    else
      syscall(SYS_munmap, devices[index].convert_mmap_buf,
	      devices[index].no_frames * V4L2_FRAME_BUF_SIZE);
    devices[index].convert_mmap_buf = MAP_FAILED;
  }

  /* Remove the fd from our list of managed fds before closing it, because as
     soon as we've done the actual close the fd maybe returned by an open in
     another thread and we don't want to intercept calls to this new fd. */
  devices[index].fd = -1;

  /* Since we've marked the fd as no longer used, and freed the resources,
     redo the close in case it was interrupted */
  do {
    result = syscall(SYS_close, fd);
  } while (result == -1 && errno == EINTR);

  V4L2_LOG("close: %d\n", fd);

  return result;
}

int v4l2_dup(int fd)
{
  int index;

  if ((index = v4l2_get_index(fd)) == -1)
    return syscall(SYS_dup, fd);

  devices[index].open_count++;

  return fd;
}

static int v4l2_check_buffer_change_ok(int index)
{
  v4l2_unmap_buffers(index);

  /* Check if the app itself still is using the stream */
  if (v4l2_buffers_mapped(index) ||
       (!(devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ) &&
	 ((devices[index].flags & V4L2_STREAMON) ||
	   devices[index].frame_queued))) {
    V4L2_LOG("v4l2_check_buffer_change_ok(): stream busy\n");
    errno = EBUSY;
    return -1;
  }

  /* We may change from convert to non conversion mode and
     v4l2_unrequest_read_buffers may change the no_frames, so free the
     convert mmap buffer */
  syscall(SYS_munmap, devices[index].convert_mmap_buf,
    devices[index].no_frames * V4L2_FRAME_BUF_SIZE);
  devices[index].convert_mmap_buf = MAP_FAILED;

  if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ) {
    V4L2_LOG("deactivating read-stream for settings change\n");
    return v4l2_deactivate_read_stream(index);
  }

  return 0;
}

int v4l2_ioctl (int fd, unsigned long int request, ...)
{
  void *arg;
  va_list ap;
  int result, converting, index, saved_err;
  int is_capture_request = 0, stream_needs_locking = 0;

  va_start (ap, request);
  arg = va_arg (ap, void *);
  va_end (ap);

  if ((index = v4l2_get_index(fd)) == -1)
    return syscall(SYS_ioctl, fd, request, arg);

  /* Appearantly the kernel and / or glibc ignore the 32 most significant bits
     when long = 64 bits, and some applications pass an int holding the req to
     ioctl, causing it to get sign extended, depending upon this behavior */
  request = (unsigned int)request;

  /* Is this a capture request and do we need to take the stream lock? */
  switch (request) {
    case VIDIOC_QUERYCAP:
      is_capture_request = 1;
      break;
    case VIDIOC_ENUM_FMT:
      if (((struct v4l2_fmtdesc *)arg)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
	  (devices[index].flags & V4L2_ENABLE_ENUM_FMT_EMULATION))
	is_capture_request = 1;
      break;
    case VIDIOC_ENUM_FRAMESIZES:
    case VIDIOC_ENUM_FRAMEINTERVALS:
      if (devices[index].flags & V4L2_ENABLE_ENUM_FMT_EMULATION)
	is_capture_request = 1;
      break;
    case VIDIOC_TRY_FMT:
      if (((struct v4l2_format *)arg)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
	is_capture_request = 1;
      break;
    case VIDIOC_S_FMT:
    case VIDIOC_G_FMT:
      if (((struct v4l2_format *)arg)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
	is_capture_request = 1;
	stream_needs_locking = 1;
      }
      break;
    case VIDIOC_REQBUFS:
      if (((struct v4l2_requestbuffers *)arg)->type ==
	  V4L2_BUF_TYPE_VIDEO_CAPTURE) {
	is_capture_request = 1;
	stream_needs_locking = 1;
      }
      break;
    case VIDIOC_QUERYBUF:
    case VIDIOC_QBUF:
    case VIDIOC_DQBUF:
      if (((struct v4l2_buffer *)arg)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
	is_capture_request = 1;
	stream_needs_locking = 1;
      }
      break;
    case VIDIOC_STREAMON:
    case VIDIOC_STREAMOFF:
      if (*((enum v4l2_buf_type *)arg) == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
	is_capture_request = 1;
	stream_needs_locking = 1;
      }
  }

  if (!is_capture_request) {
    result = syscall(SYS_ioctl, fd, request, arg);
    saved_err = errno;
    v4l2_log_ioctl(request, arg, result);
    errno = saved_err;
    return result;
  }


  if (stream_needs_locking)
    pthread_mutex_lock(&devices[index].stream_lock);

  converting = v4lconvert_needs_conversion(devices[index].convert,
			 &devices[index].src_fmt, &devices[index].dest_fmt);

  switch (request) {
    case VIDIOC_QUERYCAP:
      {
	struct v4l2_capability *cap = arg;

	result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QUERYCAP, cap);
	if (result == 0)
	  /* We always support read() as we fake it using mmap mode */
	  cap->capabilities |= V4L2_CAP_READWRITE;
      }
      break;

    case VIDIOC_ENUM_FMT:
      result = v4lconvert_enum_fmt(devices[index].convert, arg);
      break;

    case VIDIOC_ENUM_FRAMESIZES:
      result = v4lconvert_enum_framesizes(devices[index].convert, arg);
      break;

    case VIDIOC_ENUM_FRAMEINTERVALS:
      result = v4lconvert_enum_frameintervals(devices[index].convert, arg);
      break;

    case VIDIOC_TRY_FMT:
      result = v4lconvert_try_format(devices[index].convert, arg, NULL);
      break;

    case VIDIOC_S_FMT:
      {
	struct v4l2_format src_fmt, *dest_fmt = arg;

	if (!memcmp(&devices[index].dest_fmt, dest_fmt, sizeof(*dest_fmt))) {
	  result = 0;
	  break;
	}

	if (devices[index].flags & V4L2_DISABLE_CONVERSION) {
	  result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_TRY_FMT,
			   dest_fmt);
	  src_fmt = *dest_fmt;
	} else {
	  result = v4lconvert_try_format(devices[index].convert, dest_fmt,
					 &src_fmt);
	}

	if (result)
	  break;

	if (src_fmt.fmt.pix.pixelformat != dest_fmt->fmt.pix.pixelformat &&
	    v4l2_log_file) {
	  int pixfmt = src_fmt.fmt.pix.pixelformat;

	  fprintf(v4l2_log_file, "VIDIOC_S_FMT converting from: %c%c%c%c\n",
	    pixfmt & 0xff,
	    (pixfmt >> 8) & 0xff,
	    (pixfmt >> 16) & 0xff,
	    pixfmt >> 24);
	}