libv4l2.c

trident tm5600的linux驱动

	/* Maybe after try format has adjusted width/height etc, to whats
	   available nothing has changed (on the cam side) ? */
	if (!memcmp(&devices[index].src_fmt, &src_fmt, sizeof(src_fmt))) {
	  devices[index].dest_fmt = *dest_fmt;
	  result = 0;
	  break;
	}

	if ((result = v4l2_check_buffer_change_ok(index)))
	  break;

	result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_S_FMT, &src_fmt);
	if (result) {
	  saved_err = errno;
	  V4L2_LOG_ERR("setting pixformat: %s\n", strerror(errno));
	  /* Report to the app dest_fmt has not changed */
	  *dest_fmt = devices[index].dest_fmt;
	  errno = saved_err;
	  break;
	}

	devices[index].src_fmt = src_fmt;
	devices[index].dest_fmt = *dest_fmt;
      }
      break;

    case VIDIOC_G_FMT:
      {
	struct v4l2_format* fmt = arg;

	*fmt = devices[index].dest_fmt;
	result = 0;
      }
      break;

    case VIDIOC_REQBUFS:
      {
	struct v4l2_requestbuffers *req = arg;

	/* IMPROVEME (maybe?) add support for userptr's? */
	if (req->memory != V4L2_MEMORY_MMAP) {
	  errno = EINVAL;
	  result = -1;
	  break;
	}

	if ((result = v4l2_check_buffer_change_ok(index)))
	  break;

	/* No more buffers then we can manage please */
	if (req->count > V4L2_MAX_NO_FRAMES)
	  req->count = V4L2_MAX_NO_FRAMES;

	result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_REQBUFS, req);
	if (result < 0)
	  break;
	result = 0; /* some drivers return the number of buffers on success */

	devices[index].no_frames = MIN(req->count, V4L2_MAX_NO_FRAMES);
	devices[index].flags &= ~V4L2_BUFFERS_REQUESTED_BY_READ;
      }
      break;

    case VIDIOC_QUERYBUF:
      {
	struct v4l2_buffer *buf = arg;

	if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)
	  if ((result = v4l2_deactivate_read_stream(index)))
	    break;

	/* Do a real query even when converting to let the driver fill in
	   things like buf->field */
	result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QUERYBUF, buf);
	if (result || !converting)
	  break;

	buf->m.offset = V4L2_MMAP_OFFSET_MAGIC | buf->index;
	buf->length = V4L2_FRAME_BUF_SIZE;
	if (devices[index].frame_map_count[buf->index])
	  buf->flags |= V4L2_BUF_FLAG_MAPPED;
	else
	  buf->flags &= ~V4L2_BUF_FLAG_MAPPED;
      }
      break;

    case VIDIOC_QBUF:
      if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)
	if ((result = v4l2_deactivate_read_stream(index)))
	  break;

      /* With some drivers the buffers must be mapped before queuing */
      if (converting)
	if ((result = v4l2_map_buffers(index)))
	  break;

      result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QBUF, arg);
      break;

    case VIDIOC_DQBUF:
      {
	struct v4l2_buffer *buf = arg;

	if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)
	  if ((result = v4l2_deactivate_read_stream(index)))
	    break;

	if (!converting) {
	  result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_DQBUF, buf);
	  if (result) {
	    int saved_err = errno;
	    V4L2_LOG_ERR("dequeuing buf: %s\n", strerror(errno));
	    errno = saved_err;
	  }
	  break;
	}

	/* An application can do a DQBUF before mmap-ing in the buffer,
	   but we need the buffer _now_ to write our converted data
	   to it! */
	if (devices[index].convert_mmap_buf == MAP_FAILED) {
	  devices[index].convert_mmap_buf = (void *)syscall(SYS_mmap2,
						   (size_t)(
						     devices[index].no_frames *
						     V4L2_FRAME_BUF_SIZE),
						   PROT_READ|PROT_WRITE,
						   MAP_ANONYMOUS|MAP_PRIVATE,
						   -1, 0);
	  if (devices[index].convert_mmap_buf == MAP_FAILED) {
	    saved_err = errno;
	    V4L2_LOG_ERR("allocating conversion buffer\n");
	    errno = saved_err;
	    result = -1;
	    break;
	  }
	}

	result = v4l2_dequeue_and_convert(index, buf, 0, V4L2_FRAME_BUF_SIZE);
	if (result < 0)
	  break;

	buf->bytesused = result;
	buf->m.offset = V4L2_MMAP_OFFSET_MAGIC | buf->index;
	buf->length = V4L2_FRAME_BUF_SIZE;
	if (devices[index].frame_map_count[buf->index])
	  buf->flags |= V4L2_BUF_FLAG_MAPPED;
	else
	  buf->flags &= ~V4L2_BUF_FLAG_MAPPED;

	result = 0;
      }
      break;

    case VIDIOC_STREAMON:
    case VIDIOC_STREAMOFF:
      if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)
	if ((result = v4l2_deactivate_read_stream(index)))
	  break;

      if (request == VIDIOC_STREAMON)
	result = v4l2_streamon(index);
      else
	result = v4l2_streamoff(index);
      break;

    default:
      result = syscall(SYS_ioctl, fd, request, arg);
  }

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

  saved_err = errno;
  v4l2_log_ioctl(request, arg, result);
  errno = saved_err;

  return result;
}


ssize_t v4l2_read (int fd, void* dest, size_t n)
{
  ssize_t result;
  int index;
  struct v4l2_buffer buf;

  if ((index = v4l2_get_index(fd)) == -1)
    return syscall(SYS_read, fd, dest, n);

  pthread_mutex_lock(&devices[index].stream_lock);

  /* When not converting and the device supports read let the kernel handle
     it */
  if ((devices[index].flags & V4L2_SUPPORTS_READ) &&
      !v4lconvert_needs_conversion(devices[index].convert,
		   &devices[index].src_fmt, &devices[index].dest_fmt)) {
    result = syscall(SYS_read, fd, dest, n);
    goto leave;
  }

  if (!(devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)) {
    if ((devices[index].flags & V4L2_STREAMON) ||
	devices[index].frame_queued) {
      errno = EBUSY;
      result = -1;
      goto leave;
    }
    if ((result = v4l2_activate_read_stream(index)))
      goto leave;
  }

  buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  buf.memory = V4L2_MEMORY_MMAP;
  result = v4l2_dequeue_and_convert(index, &buf, dest, n);

  if (result >= 0)
    v4l2_queue_read_buffer(index, buf.index);

leave:
  pthread_mutex_unlock(&devices[index].stream_lock);

  return result;
}

void *v4l2_mmap(void *start, size_t length, int prot, int flags, int fd,
  __off64_t offset)
{
  int index;
  unsigned int buffer_index;
  void *result;

  if ((index = v4l2_get_index(fd)) == -1 ||
      /* Check if the mmap data matches our answer to QUERY_BUF, if it doesn't
	 let the kernel handle it (to allow for mmap based non capture use) */
      start || length != V4L2_FRAME_BUF_SIZE ||
      ((unsigned int)offset & ~0xFFu) != V4L2_MMAP_OFFSET_MAGIC) {
    if (index != -1)
      V4L2_LOG("Passing mmap(%p, %d, ..., %x, through to the driver\n",
	start, (int)length, (int)offset);

    if (offset & ((1 << MMAP2_PAGE_SHIFT) - 1)) {
      errno = EINVAL;
      return MAP_FAILED;
    }

    return (void *)syscall(SYS_mmap2, start, length, prot, flags, fd,
			   (__off_t)(offset >> MMAP2_PAGE_SHIFT));
  }

  pthread_mutex_lock(&devices[index].stream_lock);

  buffer_index = offset & 0xff;
  if (buffer_index >= devices[index].no_frames ||
      /* Got magic offset and not converting ?? */
      !v4lconvert_needs_conversion(devices[index].convert,
		   &devices[index].src_fmt, &devices[index].dest_fmt)) {
    errno = EINVAL;
    result = MAP_FAILED;
    goto leave;
  }

  if (devices[index].convert_mmap_buf == MAP_FAILED) {
    devices[index].convert_mmap_buf = (void *)syscall(SYS_mmap2, NULL,
					     (size_t)(
					       devices[index].no_frames *
					       V4L2_FRAME_BUF_SIZE),
					     PROT_READ|PROT_WRITE,
					     MAP_ANONYMOUS|MAP_PRIVATE,
					     -1, 0);
    if (devices[index].convert_mmap_buf == MAP_FAILED) {
      int saved_err = errno;
      V4L2_LOG_ERR("allocating conversion buffer\n");
      errno = saved_err;
      result = MAP_FAILED;
      goto leave;
    }
  }

  devices[index].frame_map_count[buffer_index]++;

  result = devices[index].convert_mmap_buf +
    buffer_index * V4L2_FRAME_BUF_SIZE;

  V4L2_LOG("Fake (conversion) mmap buf %u, seen by app at: %p\n",
    buffer_index, result);

leave:
  pthread_mutex_unlock(&devices[index].stream_lock);

  return result;
}

int v4l2_munmap(void *_start, size_t length)
{
  int index;
  unsigned int buffer_index;
  unsigned char *start = _start;

  /* Is this memory ours? */
  if (start != MAP_FAILED && length == V4L2_FRAME_BUF_SIZE) {
    for (index = 0; index < devices_used; index++)
      if (devices[index].fd != -1 &&
	  devices[index].convert_mmap_buf != MAP_FAILED &&
	  start >= devices[index].convert_mmap_buf &&
	  (start - devices[index].convert_mmap_buf) % length == 0)
	break;

    if (index != devices_used) {
      int unmapped = 0;

      pthread_mutex_lock(&devices[index].stream_lock);

      buffer_index = (start - devices[index].convert_mmap_buf) / length;

      /* Redo our checks now that we have the lock, things may have changed */
      if (devices[index].convert_mmap_buf != MAP_FAILED &&
	  start >= devices[index].convert_mmap_buf &&
	  (start - devices[index].convert_mmap_buf) % length == 0 &&
	  buffer_index < devices[index].no_frames) {
	if (devices[index].frame_map_count[buffer_index] > 0)
	  devices[index].frame_map_count[buffer_index]--;
	unmapped = 1;
      }

      pthread_mutex_unlock(&devices[index].stream_lock);

      if (unmapped) {
	V4L2_LOG("v4l2 fake buffer munmap %p, %d\n", start, (int)length);
	return 0;
      }
    }
  }

  V4L2_LOG("v4l2 unknown munmap %p, %d\n", start, (int)length);

  return syscall(SYS_munmap, _start, length);
}

/* Misc utility functions */
int v4l2_set_control(int fd, int cid, int value)
{
  struct v4l2_queryctrl qctrl = { .id = cid };
  struct v4l2_control ctrl = { .id = cid };
  int result;

  if ((result = syscall(SYS_ioctl, fd, VIDIOC_QUERYCTRL, &qctrl)))
    return result;

  if (!(qctrl.flags & V4L2_CTRL_FLAG_DISABLED) &&
      !(qctrl.flags & V4L2_CTRL_FLAG_GRABBED)) {
    if (qctrl.type == V4L2_CTRL_TYPE_BOOLEAN)
      ctrl.value = value? 1:0;
    else
      ctrl.value = (value * (qctrl.maximum - qctrl.minimum) + 32767) / 65535 +
		   qctrl.minimum;

    result = syscall(SYS_ioctl, fd, VIDIOC_S_CTRL, &ctrl);
  }

  return result;
}

int v4l2_get_control(int fd, int cid)
{
  struct v4l2_queryctrl qctrl = { .id = cid };
  struct v4l2_control ctrl = { .id = cid };

  if (syscall(SYS_ioctl, fd, VIDIOC_QUERYCTRL, &qctrl))
    return 0;

  if (qctrl.flags & V4L2_CTRL_FLAG_DISABLED)
    return 0;

  if (syscall(SYS_ioctl, fd, VIDIOC_G_CTRL, &ctrl))
    return 0;

  return ((ctrl.value - qctrl.minimum) * 65535 +
	  (qctrl.maximum - qctrl.minimum) / 2) /
	 (qctrl.maximum - qctrl.minimum);
}