libv4l1.c

trident tm5600的linux驱动

      syscall(SYS_munmap, devices[index].v4l1_frame_pointer,
	      V4L1_NO_FRAMES * V4L1_FRAME_BUF_SIZE);
    devices[index].v4l1_frame_pointer = 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;

  result = v4l2_close(fd);

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

  return result;
}

int v4l1_dup(int fd)
{
  int index;

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

  devices[index].open_count++;

  return v4l2_dup(fd);
}


int v4l1_ioctl (int fd, unsigned long int request, ...)
{
  void *arg;
  va_list ap;
  int result, index, saved_err, stream_locked = 0;

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

  if ((index = v4l1_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;

  /* do we need to take the stream lock for this ioctl? */
  switch (request) {
    case VIDIOCSPICT:
    case VIDIOCGPICT:
    case VIDIOCSWIN:
    case VIDIOCGWIN:
    case VIDIOCGMBUF:
    case VIDIOCMCAPTURE:
    case VIDIOCSYNC:
    case VIDIOC_S_FMT:
      pthread_mutex_lock(&devices[index].stream_lock);
      stream_locked = 1;
  }

  switch (request) {

    case VIDIOCGCAP:
      {
	struct video_capability *cap = arg;

	result = syscall(SYS_ioctl, fd, request, arg);

	/* override kernel v4l1 compat min / max size with our own more
	   accurate values */
	cap->minwidth  = devices[index].min_width;
	cap->minheight = devices[index].min_height;
	cap->maxwidth  = devices[index].max_width;
	cap->maxheight = devices[index].max_height;
      }
      break;

    case VIDIOCSPICT:
      {
	struct video_picture *pic = arg;

	devices[index].flags |= V4L1_PIX_FMT_TOUCHED;

	v4l2_set_control(fd, V4L2_CID_BRIGHTNESS, pic->brightness);
	v4l2_set_control(fd, V4L2_CID_HUE, pic->hue);
	v4l2_set_control(fd, V4L2_CID_CONTRAST, pic->contrast);
	v4l2_set_control(fd, V4L2_CID_SATURATION, pic->colour);
	v4l2_set_control(fd, V4L2_CID_WHITENESS, pic->whiteness);

	result = v4l1_set_format(index, devices[index].width,
		   devices[index].height, pic->palette, 0);
      }
      break;

    case VIDIOCGPICT:
      {
	struct video_picture *pic = arg;

	/* If our v4l2 pixformat has no corresponding v4l1 palette, and the
	   app has not touched the pixformat sofar, try setting a palette which
	   does (and which we emulate when necessary) so that applications
	   which just query the current format and then take whatever they get
	   will work */
	if (!(devices[index].flags & V4L1_PIX_FMT_TOUCHED) &&
	    !pixelformat_to_palette(devices[index].v4l2_pixfmt))
	  v4l1_set_format(index, devices[index].width,
				 devices[index].height,
				 VIDEO_PALETTE_RGB24,
				 (devices[index].flags &
				  V4L1_PIX_SIZE_TOUCHED) ? 0 : 1);

	devices[index].flags |= V4L1_PIX_FMT_TOUCHED;

	pic->depth = devices[index].depth;
	pic->palette = devices[index].v4l1_pal;
	pic->hue = v4l2_get_control(devices[index].fd, V4L2_CID_HUE);
	pic->colour = v4l2_get_control(devices[index].fd, V4L2_CID_SATURATION);
	pic->contrast = v4l2_get_control(devices[index].fd, V4L2_CID_CONTRAST);
	pic->whiteness = v4l2_get_control(devices[index].fd,
					   V4L2_CID_WHITENESS);
	pic->brightness = v4l2_get_control(devices[index].fd,
					   V4L2_CID_BRIGHTNESS);

	result = 0;
      }
      break;

    case VIDIOCSWIN:
      {
	struct video_window *win = arg;

	devices[index].flags |= V4L1_PIX_SIZE_TOUCHED;

	result = v4l1_set_format(index, win->width, win->height, -1, 1);
	if (result == 0) {
	  win->width  = devices[index].width;
	  win->height = devices[index].height;
	}
      }
      break;

    case VIDIOCGWIN:
      devices[index].flags |= V4L1_PIX_SIZE_TOUCHED;
      result = syscall(SYS_ioctl, fd, request, arg);
      break;

    case VIDIOCGCHAN:
      {
	struct v4l2_input input2;
	struct video_channel *chan = arg;

	if ((devices[index].flags & V4L1_SUPPORTS_ENUMINPUT) &&
	    (devices[index].flags & V4L1_SUPPORTS_ENUMSTD)) {
	  result = syscall(SYS_ioctl, fd, request, arg);
	  break;
	}

	/* Set some defaults */
	chan->tuners = 0;
	chan->flags = 0;
	chan->type = VIDEO_TYPE_CAMERA;
	chan->norm = 0;

	/* In case of no ENUMSTD support, ignore the norm member of the
	   channel struct */
	if (devices[index].flags & V4L1_SUPPORTS_ENUMINPUT) {
	  input2.index = chan->channel;
	  result = v4l2_ioctl(fd, VIDIOC_ENUMINPUT, &input2);
	  if (result == 0) {
	    snprintf(chan->name, sizeof(chan->name), "%s", (char*)input2.name);
	    if (input2.type == V4L2_INPUT_TYPE_TUNER) {
	      chan->tuners = 1;
	      chan->type = VIDEO_TYPE_TV;
	      chan->flags = VIDEO_VC_TUNER;
	    }
	  }
	  break;
	}

	/* No ENUMINPUT support, fake it (assume its a Camera in this case) */
	if (chan->channel == 0) {
	  snprintf(chan->name, sizeof(chan->name), "Camera");
	  result = 0;
	} else {
	  errno  = EINVAL;
	  result = -1;
	}
      }
      break;

    case VIDIOCSCHAN:
      {
	struct video_channel *chan = arg;
	if ((devices[index].flags & V4L1_SUPPORTS_ENUMINPUT) &&
	    (devices[index].flags & V4L1_SUPPORTS_ENUMSTD)) {
	  result = syscall(SYS_ioctl, fd, request, arg);
	  break;
	}
	/* In case of no ENUMSTD support, ignore the norm member of the
	   channel struct */
	if (devices[index].flags & V4L1_SUPPORTS_ENUMINPUT) {
	  result = v4l2_ioctl(fd, VIDIOC_S_INPUT, &chan->channel);
	  break;
	}
	/* No ENUMINPUT support, fake it (assume its a Camera in this case) */
	if (chan->channel == 0) {
	  result = 0;
	} else {
	  errno  = EINVAL;
	  result = -1;
	}
      }
      break;

    case VIDIOCGMBUF:
      /* When VIDIOCGMBUF is done, we don't necessarrily know the format the
	 application wants yet (with some apps this is passed for the first
	 time through VIDIOCMCAPTURE), so we just create an anonymous mapping
	 that should be large enough to hold any sort of frame. Note this only
	 takes virtual memory, and does not use memory until actually used. */
      {
	int i;
	struct video_mbuf *mbuf = arg;

	mbuf->size = V4L1_NO_FRAMES * V4L1_FRAME_BUF_SIZE;
	mbuf->frames = V4L1_NO_FRAMES;
	for (i = 0; i < mbuf->frames; i++) {
	  mbuf->offsets[i] = i * V4L1_FRAME_BUF_SIZE;
	}

	if (devices[index].v4l1_frame_pointer == MAP_FAILED) {
	  devices[index].v4l1_frame_pointer = (void *)syscall(SYS_mmap2, NULL,
				      (size_t)mbuf->size,
				      PROT_READ|PROT_WRITE,
				      MAP_ANONYMOUS|MAP_PRIVATE, -1, 0);
	  if (devices[index].v4l1_frame_pointer == MAP_FAILED) {
	    saved_err = errno;
	    V4L1_LOG_ERR("allocating v4l1 buffer: %s\n", strerror(errno));
	    errno = saved_err;
	    result = -1;
	    break;
	  }
	  V4L1_LOG("allocated v4l1 buffer @ %p\n",
	    devices[index].v4l1_frame_pointer);
	}
	result = 0;
      }
      break;

    case VIDIOCMCAPTURE:
      {
	struct video_mmap *map = arg;

	devices[index].flags |= V4L1_PIX_FMT_TOUCHED |
				V4L1_PIX_SIZE_TOUCHED;

	result = v4l1_set_format(index, map->width, map->height,
					map->format, 0);
      }
      break;

    case VIDIOCSYNC:
      {
	int *frame_index = arg;

	if (devices[index].v4l1_frame_pointer == MAP_FAILED ||
	    *frame_index < 0 || *frame_index >= V4L1_NO_FRAMES) {
	  errno = EINVAL;
	  result = -1;
	  break;
	}

	result = v4l2_read(devices[index].fd,
		      devices[index].v4l1_frame_pointer +
			*frame_index * V4L1_FRAME_BUF_SIZE,
		      V4L1_FRAME_BUF_SIZE);
	result = (result > 0) ? 0:result;
      }
      break;

    /* We are passing through v4l2 calls to libv4l2 for applications which are
       using v4l2 through libv4l1 (possible with the v4l1compat.so wrapper).

       So the application could be calling VIDIOC_S_FMT, in this case update
       our own bookkeeping of the cam's format. Note that this really only is
       relevant if an application is mixing and matching v4l1 and v4l2 calls,
       which is crazy, but better safe then sorry. */
    case VIDIOC_S_FMT:
      {
	struct v4l2_format *fmt2 = arg;

	result = v4l2_ioctl(fd, request, arg);

	if (result == 0 && fmt2->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
	  if (devices[index].v4l2_pixfmt != fmt2->fmt.pix.pixelformat) {
	    devices[index].v4l2_pixfmt = fmt2->fmt.pix.pixelformat;
	    devices[index].v4l1_pal =
	      pixelformat_to_palette(fmt2->fmt.pix.pixelformat);
	  }
	  devices[index].width  = fmt2->fmt.pix.width;
	  devices[index].height = fmt2->fmt.pix.height;
	}
      }
      break;

    default:
      /* Pass through libv4l2 for applications which are using v4l2 through
	 libv4l1 (this can happen with the v4l1compat.so wrapper preloaded */
      result = v4l2_ioctl(fd, request, arg);
  }

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

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

  return result;
}


ssize_t v4l1_read(int fd, void* buffer, size_t n)
{
  int index;
  ssize_t result;

  if ((index = v4l1_get_index(fd)) == -1)
    return syscall(SYS_read, fd, buffer, n);

  pthread_mutex_lock(&devices[index].stream_lock);
  result = v4l2_read(fd, buffer, n);
  pthread_mutex_unlock(&devices[index].stream_lock);

  return result;
}


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

  /* Check if the mmap data matches our answer to VIDIOCGMBUF, if not
     pass through libv4l2 for applications which are using v4l2 through
     libv4l1 (this can happen with the v4l1compat.so wrapper preloaded */
  if ((index = v4l1_get_index(fd)) == -1 || start || offset ||
      length != (V4L1_NO_FRAMES * V4L1_FRAME_BUF_SIZE))
    return v4l2_mmap(start, length, prot, flags, fd, offset);


  pthread_mutex_lock(&devices[index].stream_lock);

  /* It could be that we get called with an mmap which seems to match what
     we expect, but no VIDIOCGMBUF has been done yet, then it is certainly not
     for us so pass it through */
  if (devices[index].v4l1_frame_pointer == MAP_FAILED) {
    result = v4l2_mmap(start, length, prot, flags, fd, offset);
    goto leave;
  }

  devices[index].v4l1_frame_buf_map_count++;

  V4L1_LOG("v4l1 buffer @ %p mapped by application\n",
    devices[index].v4l1_frame_pointer);

  result = devices[index].v4l1_frame_pointer;

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

  return result;
}

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

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

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

      pthread_mutex_lock(&devices[index].stream_lock);

      /* Redo our checks now that we have the lock, things may have changed */
      if (start == devices[index].v4l1_frame_pointer) {
	if (devices[index].v4l1_frame_buf_map_count > 0)
	  devices[index].v4l1_frame_buf_map_count--;

	unmapped = 1;
      }

      pthread_mutex_unlock(&devices[index].stream_lock);

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

  V4L1_LOG("v4l1 unknown munmap %p, %d\n", start, (int)length);

  /* If not pass through libv4l2 for applications which are using v4l2 through
     libv4l1 (this can happen with the v4l1compat.so wrapper preloaded */
  return v4l2_munmap(start, length);
}