v4l2.c

Go to the documentation of this file.

/*
 * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
 * Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com
 *
 * This file is part of Paparazzi.
 *
 * Paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * Paparazzi 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, see
 * <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <string.h>
#include <assert.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <malloc.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <pthread.h>

#include "v4l2.h"

#include <sys/time.h>
#include "mcu_periph/sys_time.h"

#define CLEAR(x) memset(&(x), 0, sizeof (x))
static void *v4l2_capture_thread(void *data);

static void *v4l2_capture_thread(void *data)
{
  struct v4l2_device *dev = (struct v4l2_device *)data;
  struct v4l2_buffer buf;
  struct timeval tv;
  fd_set fds;

  while (TRUE) {
    FD_ZERO(&fds);
    FD_SET(dev->fd, &fds);

    // Set the timeout to 2 seconds
    tv.tv_sec = 2;
    tv.tv_usec = 0;

    // Wait until an image was taken, with a timeout of tv
    int sr = select(dev->fd + 1, &fds, NULL, NULL, &tv);
    uint32_t now_ts = get_sys_time_usec();

    if (sr < 0) {
      // Was interrupted by a signal
      if (EINTR == errno) { continue; }
      printf("[v4l2-capture] Select error %d on %s: %s\n", errno, dev->name, strerror(errno));
      dev->thread = (pthread_t) NULL;
      return (void *) - 1;
    } else if (sr == 0) {
      printf("[v4l2-capture] Select timeout on %s\n", dev->name);
      continue;
      //dev->thread = (pthread_t) NULL;
      //return (void *) - 2;
    }

    // Dequeue a buffer
    CLEAR(buf);
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    if (ioctl(dev->fd, VIDIOC_DQBUF, &buf) < 0) {
      printf("[v4l2-capture] Dequeue of buffer failed for %s.\n", dev->name);
      dev->thread = (pthread_t) NULL;
      return (void *) - 3;
    }
    assert(buf.index < dev->buffers_cnt);

    // Copy the timestamp
    dev->buffers[buf.index].timestamp = buf.timestamp;
    dev->buffers[buf.index].pprz_timestamp = now_ts;

    // Update the dequeued id
    // We need lock because between setting prev_idx and updating the deq_idx the deq_idx could change
    pthread_mutex_lock(&dev->mutex);
    uint16_t prev_idx = dev->buffers_deq_idx;
    dev->buffers_deq_idx = buf.index;
    pthread_mutex_unlock(&dev->mutex);

    // Enqueue the previous image if not empty
    if (prev_idx != V4L2_IMG_NONE) {
      // Enqueue the previous buffer
      CLEAR(buf);
      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory = V4L2_MEMORY_MMAP;
      buf.index = prev_idx;
      if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
        printf("[v4l2-capture] Could not enqueue %d for %s\n", prev_idx, dev->name);
      }
    }

  }
  return (void *)0;
}

bool v4l2_init_subdev(char *subdev_name, uint8_t pad, uint16_t code, struct img_size_t size)
{
  struct v4l2_subdev_format sfmt;
  CLEAR(sfmt);

  // Try to open the subdevice
  int fd = open(subdev_name, O_RDWR, 0);
  if (fd < 0) {
    printf("[v4l2] Cannot open subdevice '%s': %d, %s\n", subdev_name, errno, strerror(errno));
    return false;
  }

  // Try to get the subdevice data format settings
  if (ioctl(fd, VIDIOC_SUBDEV_G_FMT, &sfmt) < 0) {
    printf("[v4l2] Could not get subdevice data format settings of %s\n", subdev_name);
    close(fd);
    return false;
  }

  // Set the new settings
  sfmt.pad = pad;
  sfmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
  sfmt.format.width = size.w;
  sfmt.format.height = size.h;
  sfmt.format.code = code;
  sfmt.format.field = V4L2_FIELD_NONE;
  sfmt.format.colorspace = 1;

  if (ioctl(fd, VIDIOC_SUBDEV_S_FMT, &sfmt) < 0) {
    printf("[v4l2] Could not set subdevice data format settings of %s\n", subdev_name);
    close(fd);
    return false;
  }

  // Close the device
  close(fd);
  return true;
}

struct v4l2_device *v4l2_init(char *device_name, struct img_size_t size, struct crop_t crop, uint8_t buffers_cnt,
                              uint32_t _pixelformat)
{
  uint8_t i;
  struct v4l2_capability cap;
  struct v4l2_format fmt;
  struct v4l2_requestbuffers req;
  struct v4l2_fmtdesc fmtdesc;
  struct v4l2_crop crp;
  CLEAR(cap);
  CLEAR(fmt);
  CLEAR(req);
  CLEAR(fmtdesc);
  CLEAR(crp);

  // Try to open the device
  int fd = open(device_name, O_RDWR | O_NONBLOCK, 0);
  if (fd < 0) {
    printf("[v4l2] Cannot open '%s': %d, %s\n", device_name, errno, strerror(errno));
    return NULL;
  }

  // Try to fetch the capabilities of the V4L2 device
  if (ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
    printf("[v4l2] %s is no V4L2 device\n", device_name);
    close(fd);
    return NULL;
  }

  // Check if the device is capable of capturing and streaming
  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
    printf("[v4l2] %s is no V4L2 video capturing device\n", device_name);
    close(fd);
    return NULL;
  }
  if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
    printf("[v4l2] %s isn't capable of streaming (TODO: support reading)\n", device_name);
    close(fd);
    return NULL;
  }

  fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
    fmtdesc.index++;
    if(fmtdesc.pixelformat == _pixelformat)
      break;
  }

  // Accept if no format can be get
  if(fmtdesc.index != 0 && fmtdesc.pixelformat != _pixelformat) {
    printf("[v4l2] Pixelformat not available on device %s (wanted: %4X)\r\n", device_name, _pixelformat);
    return NULL;
  }

  // Set the cropping window
  crp.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  crp.c.top = crop.y;
  crp.c.left = crop.x;
  crp.c.width = crop.w;
  crp.c.height = crop.h;

  // Only crop when needed
  if(crop.x != 0 || crop.y != 0 || crop.w != size.w || crop.h != size.h) {
    if (ioctl(fd, VIDIOC_S_CROP, &crp) < 0) {
      printf("[v4l2] Could not set crop window of %s\n", device_name);
      close(fd);
      return NULL;
    }
  }

  // Set the format settings
  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmt.fmt.pix.width = size.w;
  fmt.fmt.pix.height = size.h;
  fmt.fmt.pix.pixelformat = _pixelformat;
  fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
  fmt.fmt.pix.field = V4L2_FIELD_NONE;

  if (ioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
    printf("[v4l2] Could not set data format settings of %s\n", device_name);
    close(fd);
    return NULL;
  }

  // Request MMAP buffers
  req.count = buffers_cnt;
  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory = V4L2_MEMORY_MMAP;
  if (ioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
    printf("[v4l2] %s Does not support memory mapping\n", device_name);
    close(fd);
    return NULL;
  }

  // Allocate memory for the memory mapped buffers
  struct v4l2_img_buf *buffers = calloc(req.count, sizeof(struct v4l2_img_buf));
  if (buffers == NULL) {
    printf("[v4l2] Not enough memory for %s to initialize %d MMAP buffers\n", device_name, req.count);
    close(fd);
    return NULL;
  }

  // Go trough the buffers and initialize them
  for (i = 0; i < req.count; ++i) {
    struct v4l2_buffer buf;
    CLEAR(buf);

    // Request the buffer information
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = i;
    if (ioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
      printf("[v4l2] Querying buffer %d from %s failed\n", i, device_name);
      free(buffers);
      close(fd);
      return NULL;
    }

    //  Map the buffer
    buffers[i].length = buf.length;
    buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
    if (MAP_FAILED == buffers[i].buf) {
      printf("[v4l2] Mapping buffer %d with length %d from %s failed\n", i, buf.length, device_name);
      free(buffers);
      close(fd);
      return NULL;
    }
  }

  // Create the device only when everything succeeded
  struct v4l2_device *dev = (struct v4l2_device *)malloc(sizeof(struct v4l2_device));
  CLEAR(*dev);
  dev->name = strdup(device_name); // NOTE: needs to be freed
  dev->fd = fd;
  dev->w = size.w;
  dev->h = size.h;
  dev->buffers_cnt = req.count;
  dev->buffers = buffers;
  return dev;
}

void v4l2_image_get(struct v4l2_device *dev, struct image_t *img)
{
  uint16_t img_idx = V4L2_IMG_NONE;

  // Continu to wait for an image
  while (img_idx == V4L2_IMG_NONE) {
    // We first check if the deq_idx is ok, this reduces the amount of locks
    if (dev->buffers_deq_idx != V4L2_IMG_NONE) {
      pthread_mutex_lock(&dev->mutex);

      // We need to check it here again, because it could be changed
      if (dev->buffers_deq_idx != V4L2_IMG_NONE) {
        img_idx = dev->buffers_deq_idx;
        dev->buffers_deq_idx = V4L2_IMG_NONE;
      }

      pthread_mutex_unlock(&dev->mutex);
    }
  }

  // Set the image
  img->type = IMAGE_YUV422;
  img->w = dev->w;
  img->h = dev->h;
  img->buf_idx = img_idx;
  img->buf_size = dev->buffers[img_idx].length;
  img->buf = dev->buffers[img_idx].buf;
  img->ts = dev->buffers[img_idx].timestamp;
  img->pprz_ts =  dev->buffers[img_idx].pprz_timestamp;
}

bool v4l2_image_get_nonblock(struct v4l2_device *dev, struct image_t *img)
{
  uint16_t img_idx = V4L2_IMG_NONE;

  // Try to get the current image
  pthread_mutex_lock(&dev->mutex);
  if (dev->buffers_deq_idx != V4L2_IMG_NONE) {
    img_idx = dev->buffers_deq_idx;
    dev->buffers_deq_idx = V4L2_IMG_NONE;
  }
  pthread_mutex_unlock(&dev->mutex);

  // Check if we really got an image
  if (img_idx == V4L2_IMG_NONE) {
    return false;
  } else {
    // Set the image
    img->type = IMAGE_YUV422;
    img->w = dev->w;
    img->h = dev->h;
    img->buf_idx = img_idx;
    img->buf_size = dev->buffers[img_idx].length;
    img->buf = dev->buffers[img_idx].buf;
    img->ts = dev->buffers[img_idx].timestamp;
    img->pprz_ts = dev->buffers[img_idx].pprz_timestamp;
    return true;
  }
}

void v4l2_image_free(struct v4l2_device *dev, struct image_t *img)
{
  struct v4l2_buffer buf;

  // Enqueue the buffer
  CLEAR(buf);
  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  buf.memory = V4L2_MEMORY_MMAP;
  buf.index = img->buf_idx;
  if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
    printf("[v4l2] Could not enqueue %d for %s\n", img->buf_idx, dev->name);
  }
}

bool v4l2_start_capture(struct v4l2_device *dev)
{
  uint8_t i;
  enum v4l2_buf_type type;

  // Check if not already running
  if (dev->thread != (pthread_t)NULL) {
    printf("[v4l2] There is already a capturing thread running for %s\n", dev->name);
    return false;
  }

  // Enqueue all buffers
  dev->buffers_deq_idx = V4L2_IMG_NONE;
  for (i = 0; i < dev->buffers_cnt; ++i) {
    struct v4l2_buffer buf;

    CLEAR(buf);
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = i;
    if (ioctl(dev->fd, VIDIOC_QBUF, &buf) < 0) {
      printf("[v4l2] Could not enqueue buffer %d during start capture for %s\n", i, dev->name);
      return false;
    }
  }

  // Start the stream
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (ioctl(dev->fd, VIDIOC_STREAMON, &type) < 0) {
    printf("[v4l2] Could not start stream of %s, %d %s\n", dev->name, errno, strerror(errno));
    return false;
  }

  //Start the capturing thread
  int rc = pthread_create(&dev->thread, NULL, v4l2_capture_thread, dev);
  if (rc < 0) {
    printf("[v4l2] Could not start capturing thread for %s (return code: %d)\n", dev->name, rc);

    // Stop the stream
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(dev->fd, VIDIOC_STREAMOFF, &type) < 0) {
      printf("[v4l2] Could not stop stream of %s\n", dev->name);
    }

    // Reset the thread
    dev->thread = (pthread_t) NULL;
    return false;
  }

  return true;
}

bool v4l2_stop_capture(struct v4l2_device *dev)
{
  enum v4l2_buf_type type;

  // First check if still running
  if (dev->thread == (pthread_t) NULL) {
    printf("[v4l2] Already stopped capture for %s\n", dev->name);
    return false;
  }

  // Stop the stream
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (ioctl(dev->fd, VIDIOC_STREAMOFF, &type) < 0) {
    printf("[v4l2] Could not stop stream of %s\n", dev->name);
    return false;
  }

  // Stop the thread
  if (pthread_cancel(dev->thread) < 0) {
    printf("[v4l2] Could not cancel thread for %s\n", dev->name);
    return false;
  }

  // Wait for the thread to be finished
  pthread_join(dev->thread, NULL);
  dev->thread = (pthread_t) NULL;
  return true;
}

void v4l2_close(struct v4l2_device *dev)
{
  uint8_t i;

  // Stop capturing (ignore result as it may already be stopped)
  v4l2_stop_capture(dev);

  // Unmap all buffers
  for (i = 0; i < dev->buffers_cnt; ++i) {
    if (munmap(dev->buffers[i].buf, dev->buffers[i].length) < 0) {
      printf("[v4l2] Could not unmap buffer %d for %s\n", i, dev->name);
    }
  }

  // Close the file pointer and free all memory
  close(dev->fd);
  free(dev->name);
  free(dev);
}