forge-steel/src/video.c

#ifdef VIDEO_IN

#include <errno.h>
#include <fcntl.h>
#include <linux/videodev2.h>
#include <log.h>
#include <pthread.h>
#include <stdbool.h>
#include <string.h>
#include <sys/ioctl.h>
#include <unistd.h>

#include "types.h"

#include "config.h"
#include "timer.h"
#include "video.h"

static const unsigned int pixel_format = V4L2_PIX_FMT_YUYV;
static const enum v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

static void ioctl_error(VideoCapture *video_capture, const char *operation,
                        const char *default_msg) {
  if (errno == EINVAL) {
    log_warn("(%s) %s -> EINVAL: %s", video_capture->name, operation,
             default_msg);
  } else if (errno == EAGAIN) {
    log_warn("(%s) %s -> EAGAIN: device state invalid", operation,
             video_capture->name);
  } else if (errno == EBADF) {
    log_warn("(%s) %s -> EBADF: file descriptor invalid", operation,
             video_capture->name);
  } else if (errno == EBUSY) {
    log_warn("(%s) %s -> EBUSY: device is busy", video_capture->name,
             operation);
  } else if (errno == EFAULT) {
    log_warn("(%s) %s -> EFAULT: invalid pointer", video_capture->name,
             operation);
  } else if (errno == ENODEV) {
    log_warn("(%s) %s -> ENODEV: device not found", video_capture->name,
             operation);
  } else if (errno == ENOMEM) {
    log_warn("(%s) %s -> ENOMEM: not enough memory", video_capture->name,
             operation);
  } else if (errno == ENOTTY) {
    log_warn("(%s) %s -> ENOTTY: ioctl not supported by file descriptor",
             video_capture->name, operation);
  } else if (errno == ENOSPC) {
    log_warn("(%s) %s -> ENOSPC: USB bandwidth error", video_capture->name,
             operation);
  } else if (errno == EPERM) {
    log_warn("(%s) %s -> EPERM: permission denied", video_capture->name,
             operation);
  } else if (errno == EIO) {
    log_warn("(%s) %s -> EIO: I/O error", video_capture->name, operation);
  } else if (errno == ENXIO) {
    log_warn("(%s) %s -> ENXIO: no device exists", video_capture->name,
             operation);
  } else if (errno == EPIPE) {
    log_warn("(%s) %s -> EPIPE: pipeline error", video_capture->name,
             operation);
  } else if (errno == ENOLINK) {
    log_warn("(%s) %s -> ENOLINK: pipeline configuration invalid for Media "
             "Controller interface",
             video_capture->name, operation);
  } else {
    log_error("(%s) %s unknown error %d", video_capture->name, operation,
              errno);
  }
  video_capture->error = true;
}

static void open_device(VideoCapture *video_capture, const char *name) {
  strlcpy(video_capture->name, name, STR_LEN);
  video_capture->error = false;
  video_capture->fd = -1;

  video_capture->fd = open(name, O_RDWR | O_NONBLOCK);
  if (video_capture->fd == -1) {
    log_warn("(%s) Cannot open device", name);
    video_capture->error = true;
  }
}

static bool check_caps(VideoCapture *video_capture) {
  struct v4l2_capability cap;

  memset(&cap, 0, sizeof(cap));

  if (ioctl(video_capture->fd, VIDIOC_QUERYCAP, &cap) == -1) {
    ioctl_error(video_capture, "VIDIOC_QUERYCAP", "Not a V4L2 device");
    return false;
  }

  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
    log_warn("(%s) Not a video capture device", video_capture->name);
    video_capture->error = true;
    return false;
  }

  if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
    log_warn("(%s) No streaming i/o support", video_capture->name);
    video_capture->error = true;
    return false;
  }

  return true;
}

static bool get_available_sizes(VideoCapture *video_capture,
                                unsigned int preferred_height) {
  struct v4l2_frmsizeenum fmt_enum;
  unsigned int index;
  bool found = false;

  memset(&fmt_enum, 0, sizeof(fmt_enum));

  index = 0;
  fmt_enum.index = index;
  fmt_enum.pixel_format = pixel_format;

  found = false;
  video_capture->width = 0;
  video_capture->height = 0;

  while (ioctl(video_capture->fd, VIDIOC_ENUM_FRAMESIZES, &fmt_enum) == 0) {
    if (fmt_enum.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
      log_trace("(%s) %d: %dx%d", video_capture->name, index,
                fmt_enum.discrete.width, fmt_enum.discrete.height);

      if (fmt_enum.discrete.height == preferred_height) {
        video_capture->height = preferred_height;
        found = true;
        if (video_capture->width == 0 ||
            video_capture->width < fmt_enum.discrete.width) {
          video_capture->width = fmt_enum.discrete.width;
        }
      } else if (fmt_enum.discrete.height < preferred_height) {
        if (!found || fmt_enum.discrete.height > video_capture->height) {
          video_capture->height = fmt_enum.discrete.height;
          video_capture->width = fmt_enum.discrete.width;
          found = true;
        }
      } else if (video_capture->height == 0) {
        video_capture->height = fmt_enum.discrete.height;
        video_capture->width = fmt_enum.discrete.width;
      }
    }

    memset(&fmt_enum, 0, sizeof(fmt_enum));
    fmt_enum.index = ++index;
    fmt_enum.pixel_format = pixel_format;
  }

  if (video_capture->height == 0) {
    log_warn("(%s) No format found", video_capture->name);
    video_capture->error = true;
    return false;
  }

  return true;
}

static bool set_format(VideoCapture *video_capture) {
  struct v4l2_format fmt;

  memset(&fmt, 0, sizeof(fmt));

  fmt.type = buf_type;
  fmt.fmt.pix.width = video_capture->width;
  fmt.fmt.pix.height = video_capture->height;
  fmt.fmt.pix.pixelformat = pixel_format;
  fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;

  if (ioctl(video_capture->fd, VIDIOC_S_FMT, &fmt) == -1) {
    ioctl_error(video_capture, "VIDIOC_S_FMT",
                "Requested buffer type not supported");
    return false;
  }

  video_capture->width = fmt.fmt.pix.width;
  video_capture->height = fmt.fmt.pix.height;
  video_capture->pixelformat = fmt.fmt.pix.pixelformat;
  video_capture->bytesperline = fmt.fmt.pix.bytesperline;

  log_info("(%s) Format fourcc:  %c%c%c%c", video_capture->name,
           fmt.fmt.pix.pixelformat, fmt.fmt.pix.pixelformat >> 8,
           fmt.fmt.pix.pixelformat >> 16, fmt.fmt.pix.pixelformat >> 24);
  log_info("(%s) Resolution:  %dx%d", video_capture->name, fmt.fmt.pix.width,
           fmt.fmt.pix.height);

  return true;
}

static bool request_buffers(VideoCapture *video_capture,
                            unsigned int buffer_count) {
  struct v4l2_requestbuffers reqbuf;

  memset(&reqbuf, 0, sizeof(reqbuf));

  reqbuf.type = buf_type;
  reqbuf.memory = V4L2_MEMORY_MMAP;
  reqbuf.count = buffer_count;

  if (ioctl(video_capture->fd, VIDIOC_REQBUFS, &reqbuf) == -1) {
    ioctl_error(video_capture, "VIDIOC_REQBUFS",
                "Buffer type or I/O method not supported");
    return false;
  }

  log_info("(%s) V4L2 Buffer Count:  %d", video_capture->name, reqbuf.count);

  video_capture->buf_count = reqbuf.count;

  return true;
}

static bool export_buffer(VideoCapture *video_capture, int *fd,
                          unsigned int index) {
  struct v4l2_exportbuffer expbuf;

  *fd = -1;

  memset(&expbuf, 0, sizeof(expbuf));

  expbuf.type = buf_type;
  expbuf.index = index;
  expbuf.flags = O_RDONLY;

  if (ioctl(video_capture->fd, VIDIOC_EXPBUF, &expbuf) == -1) {
    ioctl_error(
        video_capture, "VIDIOC_EXPBUF",
        "A queue is not in MMAP mode or DMABUF exporting is not supported");
    return false;
  }

  *fd = expbuf.fd;

  return true;
}

static bool export_buffers(VideoCapture *video_capture) {
  unsigned int i;

  for (i = 0; i < video_capture->buf_count; i++) {
    if (!export_buffer(video_capture, &video_capture->exp_fd[i], i)) {
      return false;
    }
  }

  return true;
}

static bool open_stream(VideoCapture *video_capture) {
  if (ioctl(video_capture->fd, VIDIOC_STREAMON, &buf_type) == -1) {
    ioctl_error(
        video_capture, "VIDIOC_STREAMON",
        "Buffer type not supported or no buffer allocated or enqueued yet");
    return false;
  }

  return true;
}

static void create_image_buffer(const VideoCapture *video_capture,
                                struct v4l2_buffer *buf, unsigned int index) {
  memset(buf, 0, sizeof(*buf));

  buf->type = buf_type;
  buf->memory = V4L2_MEMORY_MMAP;
  buf->index = index;

  ioctl(video_capture->fd, VIDIOC_QBUF, buf);
}

static void create_image_buffers(VideoCapture *video_capture) {
  unsigned int i;

  for (i = 0; i < video_capture->buf_count; i++) {
    create_image_buffer(video_capture, &video_capture->buf[i], i);
  }
}

static void close_stream(const VideoCapture *video_capture) {
  ioctl(video_capture->fd, VIDIOC_STREAMOFF, &buf_type);
}

static unsigned int read_video(VideoCapture *video_capture) {
  struct v4l2_capability cap;

  if (ioctl(video_capture->fd, VIDIOC_DQBUF,
            &video_capture->buf[video_capture->buf_index]) != -1) {
    ioctl(video_capture->fd, VIDIOC_QBUF,
          &video_capture->buf[video_capture->buf_index]);
    video_capture->buf_index =
        (video_capture->buf_index + 1) % video_capture->buf_count;
    return true;
  }

  if (ioctl(video_capture->fd, VIDIOC_QUERYCAP, &cap) == -1) {
    video_capture->error = true;
  }

  return false;
}

void video_init(VideoCapture *video_capture, const char *name,
                unsigned int preferred_height, unsigned int buffer_count) {
  open_device(video_capture, name);

  if (video_capture->error) {
    return;
  }

  if (!check_caps(video_capture)) {
    return;
  }

  if (!get_available_sizes(video_capture, preferred_height)) {
    return;
  }

  if (!set_format(video_capture)) {
    return;
  }

  if (!request_buffers(video_capture, buffer_count)) {
    return;
  }

  if (!export_buffers(video_capture)) {
    return;
  }

  if (!open_stream(video_capture)) {
    return;
  }

  create_image_buffers(video_capture);
}

void *video_background_read(void *args) {
  VideoBackgroundReadArgs *process_args = (VideoBackgroundReadArgs *)args;
  VideoCapture *video_capture = process_args->capture;
  Context *context = process_args->context;
  int input_index = process_args->input_index;
  bool trace_fps = process_args->trace_fps;
  Timer timer;
  double fps;
  bool result;

  log_info("(%s) background acquisition started", video_capture->name);
  timer_init(&timer, 30);

  while (!context->stop && !video_capture->error) {
    result = read_video(video_capture);

    if (result && timer_inc(&timer)) {
      fps = timer_reset(&timer);

      context->input_fps[input_index] = (unsigned int)round(fps);
      if (trace_fps) {
        log_trace("(%s) %.2ffps", video_capture->name, fps);
      }
    }

    if (result) {
      context->input_index[input_index] = video_capture->buf_index;
    }
  }
  if (context->stop) {
    log_info("(%s) background acquisition stopped by main thread",
             video_capture->name);
  } else {
    log_info("(%s) background acquisition stopped after error",
             video_capture->name);
    video_capture->disconnected = true;
    context->input_formats[input_index] = 0;
  }
  free(process_args);
  pthread_exit(NULL);
}

void video_free(const VideoCapture *video_capture) {
  unsigned int i;

  close_stream(video_capture);

  for (i = 0; i < video_capture->buf_count; i++) {
    close(video_capture->exp_fd[i]);
  }

  if (video_capture->fd != -1) {
    close(video_capture->fd);
  }
}

#endif /* VIDEO_IN */