libv4l2.c

trident tm5600的linux驱动

/*
#             (C) 2008 Hans de Goede <[e-mail]>

# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation; either version 2.1 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
# Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

/* MAKING CHANGES TO THIS FILE??   READ THIS FIRST!!!

   This file implements libv4l2, which offers v4l2_ prefixed versions of
   open/close/etc. The API is 100% the same as directly opening /dev/videoX
   using regular open/close/etc, the big difference is that format conversion
   is done if necessary when capturing. That is if you (try to) set a capture
   format which is not supported by the cam, but is supported by libv4lconvert,
   then the try_fmt / set_fmt will succeed as if the cam supports the format
   and on dqbuf / read the data will be converted for you and returned in
   the request format.

   Important note to people making changes to this file: All functions
   (v4l2_close, v4l2_ioctl, etc.) are designed to function as their regular
   counterpart when they get passed a fd that is not "registered" by libv4l2,
   there are 2 reasons for this:
   1) This allows us to get completely out of the way when dealing with non
      capture devices.
   2) libv4l2 is the base of the v4l2convert.so wrapper lib, which is a .so
      which can be LD_PRELOAD-ed and the overrules the libc's open/close/etc,
      and when opening /dev/videoX or /dev/v4l/ calls v4l2_open.  Because we
      behave as the regular counterpart when the fd is not known (instead of say
      throwing an error), v4l2convert.so can simply call the v4l2_ prefixed
      function for all wrapped functions (except for v4l2_open which will fail
      when not called on a v4l2 device). This way the wrapper does not have to
      keep track of which fd's are being handled by libv4l2, as libv4l2 already
      keeps track of this itself.

      This also means that libv4l2 may not use any of the regular functions
      it mimics, as for example open could be a symbol in v4l2convert.so, which
      in turn will call v4l2_open, so therefor v4l2_open (for example) may not
      use the regular open()!

   Another important note: libv4l2 does conversion for capture usage only, if
   any calls are made which are passed a v4l2_buffer or v4l2_format with a
   v4l2_buf_type which is different from V4L2_BUF_TYPE_VIDEO_CAPTURE, then
   the v4l2_ methods behave exactly the same as their regular counterparts.
   When modifications are made, one should be carefull that this behavior is
   preserved.
*/
#include <errno.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <syscall.h>
#include <fcntl.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include "libv4l2.h"
#include "libv4l2-priv.h"

/* Note these flags are stored together with the flags passed to v4l2_fd_open()
   in v4l2_dev_info's flags member, so care should be taken that the do not
   use the same bits! */
#define V4L2_STREAMON			0x0100
#define V4L2_BUFFERS_REQUESTED_BY_READ	0x0200
#define V4L2_STREAM_CONTROLLED_BY_READ	0x0400
#define V4L2_SUPPORTS_READ		0x0800

#define V4L2_MMAP_OFFSET_MAGIC      0xABCDEF00u

static pthread_mutex_t v4l2_open_mutex = PTHREAD_MUTEX_INITIALIZER;
static struct v4l2_dev_info devices[V4L2_MAX_DEVICES] = { { .fd = -1 },
  { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 },
  { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 },
  { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 }};
static int devices_used = 0;


static int v4l2_request_read_buffers(int index)
{
  int result;
  struct v4l2_requestbuffers req;

  /* Note we re-request the buffers if they are already requested as the format
     and thus the needed buffersize may have changed. */
  req.count = (devices[index].no_frames)? devices[index].no_frames:
					  devices[index].nreadbuffers;
  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory = V4L2_MEMORY_MMAP;
  if ((result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_REQBUFS, &req)) < 0){
    int saved_err = errno;
    V4L2_LOG_ERR("requesting %u buffers: %s\n", req.count, strerror(errno));
    errno = saved_err;
    return result;
  }

  if (!devices[index].no_frames && req.count)
    devices[index].flags |= V4L2_BUFFERS_REQUESTED_BY_READ;

  devices[index].no_frames = MIN(req.count, V4L2_MAX_NO_FRAMES);
  return 0;
}

static void v4l2_unrequest_read_buffers(int index)
{
  struct v4l2_requestbuffers req;

  if (!(devices[index].flags & V4L2_BUFFERS_REQUESTED_BY_READ) ||
      devices[index].no_frames == 0)
    return;

  /* (Un)Request buffers, note not all driver support this, and those
     who do not support it don't need it. */
  req.count = 0;
  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory = V4L2_MEMORY_MMAP;
  if(syscall(SYS_ioctl, devices[index].fd, VIDIOC_REQBUFS, &req) < 0)
    return;

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

static int v4l2_map_buffers(int index)
{
  int result = 0;
  unsigned int i;
  struct v4l2_buffer buf;

  for (i = 0; i < devices[index].no_frames; i++) {
    if (devices[index].frame_pointers[i] != MAP_FAILED)
      continue;

    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = i;
    result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QUERYBUF, &buf);
    if (result) {
      int saved_err = errno;
      V4L2_LOG_ERR("querying buffer %u: %s\n", i, strerror(errno));
      errno = saved_err;
      break;
    }

    devices[index].frame_pointers[i] = (void *)syscall(SYS_mmap2, NULL,
      (size_t)buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, devices[index].fd,
      (__off_t)(buf.m.offset >> MMAP2_PAGE_SHIFT));
    if (devices[index].frame_pointers[i] == MAP_FAILED) {
      int saved_err = errno;
      V4L2_LOG_ERR("mmapping buffer %u: %s\n", i, strerror(errno));
      errno = saved_err;
      result = -1;
      break;
    }
    V4L2_LOG("mapped buffer %u at %p\n", i,
      devices[index].frame_pointers[i]);

    devices[index].frame_sizes[i] = buf.length;
  }

  return result;
}

static void v4l2_unmap_buffers(int index)
{
  unsigned int i;

  /* unmap the buffers */
  for (i = 0; i < devices[index].no_frames; i++) {
    if (devices[index].frame_pointers[i] != MAP_FAILED) {
      syscall(SYS_munmap, devices[index].frame_pointers[i],
	      devices[index].frame_sizes[i]);
      devices[index].frame_pointers[i] = MAP_FAILED;
      V4L2_LOG("unmapped buffer %u\n", i);
    }
  }
}

static int v4l2_streamon(int index)
{
  int result;
  enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (!(devices[index].flags & V4L2_STREAMON)) {
    if ((result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_STREAMON,
			  &type))) {
      int saved_err = errno;
      V4L2_LOG_ERR("turning on stream: %s\n", strerror(errno));
      errno = saved_err;
      return result;
    }
    devices[index].flags |= V4L2_STREAMON;
  }

  return 0;
}

static int v4l2_streamoff(int index)
{
  int result;
  enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (devices[index].flags & V4L2_STREAMON) {
    if ((result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_STREAMOFF,
			  &type))) {
      int saved_err = errno;
      V4L2_LOG_ERR("turning off stream: %s\n", strerror(errno));
      errno = saved_err;
      return result;
    }
    devices[index].flags &= ~V4L2_STREAMON;

    /* Stream off also unqueues all our buffers! */
    devices[index].frame_queued = 0;
  }

  return 0;
}

static int v4l2_queue_read_buffer(int index, int buffer_index)
{
  int result;
  struct v4l2_buffer buf;

  if (devices[index].frame_queued & (1 << buffer_index))
    return 0;

  memset(&buf, 0, sizeof(buf));
  buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  buf.memory = V4L2_MEMORY_MMAP;
  buf.index  = buffer_index;
  if ((result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QBUF, &buf))) {
    int saved_err = errno;
    V4L2_LOG_ERR("queuing buf %d: %s\n", buffer_index, strerror(errno));
    errno = saved_err;
    return result;
  }

  devices[index].frame_queued |= 1 << buffer_index;
  return 0;
}

static int v4l2_dequeue_and_convert(int index, struct v4l2_buffer *buf,
  unsigned char *dest, int dest_size)
{
  const int max_tries = 10;
  int result, tries = max_tries;

  /* Make sure we have the real v4l2 buffers mapped */
  if ((result = v4l2_map_buffers(index)))
    return result;

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

    devices[index].frame_queued &= ~(1 << buf->index);

    result = v4lconvert_convert(devices[index].convert,
	   &devices[index].src_fmt, &devices[index].dest_fmt,
	   devices[index].frame_pointers[buf->index],
	   buf->bytesused, dest ? dest : (devices[index].convert_mmap_buf +
	     buf->index * V4L2_FRAME_BUF_SIZE), dest_size);

    if (result < 0) {
      int saved_err = errno;

      if(errno == EAGAIN)
	V4L2_LOG("warning error while converting frame data: %s\n",
	  v4lconvert_get_error_message(devices[index].convert));
      else
	V4L2_LOG_ERR("converting / decoding frame data: %s\n",
	  v4lconvert_get_error_message(devices[index].convert));

      v4l2_queue_read_buffer(index, buf->index);
      errno = saved_err;
    }
    tries--;
  } while (result < 0 && errno == EAGAIN && tries);

  if (result < 0 && errno == EAGAIN) {
    V4L2_LOG_ERR("got %d consecutive frame decode errors, last error: %s\n",
      max_tries, v4lconvert_get_error_message(devices[index].convert));
  }

  return result;
}

static int v4l2_queue_read_buffers(int index)
{
  unsigned int i;
  int last_error = EIO, queued = 0;

  for (i = 0; i < devices[index].no_frames; i++) {
    /* Don't queue unmapped buffers (should never happen) */
    if (devices[index].frame_pointers[i] != MAP_FAILED) {
      if (v4l2_queue_read_buffer(index, i)) {
	last_error = errno;
	continue;
      }
      queued++;
    }
  }

  if (!queued) {
    errno = last_error;
    return -1;
  }
  return 0;
}

static int v4l2_activate_read_stream(int index)
{
  int result;

  if ((result = v4l2_request_read_buffers(index)))
    return result;

  if ((result = v4l2_map_buffers(index)))
    return result;

  if ((result = v4l2_queue_read_buffers(index)))
    return result;

  devices[index].flags |= V4L2_STREAM_CONTROLLED_BY_READ;

  return result = v4l2_streamon(index);
}

static int v4l2_deactivate_read_stream(int index)
{
  int result;

  if ((result = v4l2_streamoff(index)))
    return result;

  /* No need to unqueue our buffers, streamoff does that for us */

  v4l2_unmap_buffers(index);

  v4l2_unrequest_read_buffers(index);

  devices[index].flags &= ~V4L2_STREAM_CONTROLLED_BY_READ;

  return 0;
}

static int v4l2_buffers_mapped(int index)
{
  unsigned int i;

  if (devices[index].src_fmt.fmt.pix.pixelformat ==
      devices[index].dest_fmt.fmt.pix.pixelformat) {
    /* Normal (no conversion) mode */
    struct v4l2_buffer buf;

    for (i = 0; i < devices[index].no_frames; i++) {
      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory = V4L2_MEMORY_MMAP;
      buf.index = i;
      if (syscall(SYS_ioctl, devices[index].fd, VIDIOC_QUERYBUF, &buf)) {
	int saved_err = errno;
	V4L2_LOG_ERR("querying buffer %u: %s\n", i, strerror(errno));
	errno = saved_err;
	break;
      }