nuttx-apps/system/nxcamera/nxcamera.c

/****************************************************************************
 * apps/system/nxcamera/nxcamera.c
 *
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.  The
 * ASF licenses this file to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance with the
 * License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
 * License for the specific language governing permissions and limitations
 * under the License.
 *
 ****************************************************************************/

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <nuttx/config.h>

#include <assert.h>
#include <debug.h>
#include <dirent.h>
#include <errno.h>
#include <fcntl.h>
#include <pthread.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <nuttx/queue.h>
#include <nuttx/video/video.h>
#include <nuttx/video/fb.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/param.h>
#include <sys/types.h>

#include <system/nxcamera.h>

#ifdef CONFIG_LIBYUV
#  include <libyuv.h>
#endif

/****************************************************************************
 * Pre-processor Definitions
 ****************************************************************************/

#define NXCAMERA_STATE_IDLE      0
#define NXCAMERA_STATE_STREAMING 1
#define NXCAMERA_STATE_LOOPING   2
#define NXCAMERA_STATE_PAUSED    3

#define convert_frame            ConvertToARGB

/****************************************************************************
 * Private Functions
 ****************************************************************************/

static int show_image(FAR struct nxcamera_s *pcam, FAR v4l2_buffer_t *buf)
{
#ifdef CONFIG_LIBYUV
  return convert_frame(pcam->bufs[buf->index],
                       pcam->buf_sizes[buf->index],
                       pcam->display_pinfo.fbmem,
                       pcam->display_pinfo.stride,
                       0,
                       0,
                       pcam->fmt.fmt.pix.width,
                       pcam->fmt.fmt.pix.height,
                       pcam->fmt.fmt.pix.width,
                       pcam->fmt.fmt.pix.height,
                       0,
                       pcam->fmt.fmt.pix.pixelformat);
#else
  uint32_t *pbuf = pcam->bufs[buf->index];
  vinfo("show image from %p: %" PRIx32 " %" PRIx32, pbuf, pbuf[0], pbuf[1]);
  return 0;
#endif
}

/****************************************************************************
 * Name: nxcamera_opendevice
 *
 *   nxcamera_opendevice() tries to open the preferred devices as specified.
 *
 * Return:
 *    OK        if compatible device opened (searched or preferred)
 *    -ENODEV   if no compatible device opened.
 *    -ENOENT   if preferred device couldn't be opened.
 *
 ****************************************************************************/

static int nxcamera_opendevice(FAR struct nxcamera_s *pcam)
{
  int errcode;

  if (pcam->capturedev[0] != '\0')
    {
      pcam->capture_fd = open(pcam->capturedev, O_RDWR);
      if (pcam->capture_fd == -1)
        {
          errcode = errno;
          DEBUGASSERT(errcode > 0);

          verr("ERROR: Failed to open pcam->capturedev %d\n", -errcode);
          return -errcode;
        }

      if (pcam->displaydev[0] != '\0')
        {
          pcam->display_fd = open(pcam->displaydev, O_RDWR);
          if (pcam->display_fd == -1)
            {
              errcode = errno;
              DEBUGASSERT(errcode > 0);

              close(pcam->capture_fd);
              pcam->capture_fd = -1;
              verr("ERROR: Failed to open pcam->displaydev %d\n", -errcode);
              return -errcode;
            }

          errcode = ioctl(pcam->display_fd, FBIOGET_PLANEINFO,
                          ((uintptr_t)&pcam->display_pinfo));

          if (errcode == OK)
            {
              pcam->display_pinfo.fbmem = mmap(NULL,
                                               pcam->display_pinfo.fblen,
                                               PROT_READ | PROT_WRITE,
                                               MAP_SHARED | MAP_FILE,
                                               pcam->display_fd,
                                               0);
            }

          if (errcode < 0 || pcam->display_pinfo.fbmem == MAP_FAILED)
            {
              errcode = errno;
              close(pcam->capture_fd);
              close(pcam->display_fd);
              verr("ERROR: ioctl(FBIOGET_PLANEINFO) failed: %d\n", -errcode);
              return -errcode;
            }

          return OK;
        }
      else
        {
          /* TODO: Add file output */

          return -ENOTSUP;
        }
    }

  return -ENODEV;
}

/****************************************************************************
 * Name: nxcameraer_jointhread
 ****************************************************************************/

static void nxcamera_jointhread(FAR struct nxcamera_s *pcam)
{
  FAR void *value;
  int id = 0;

  if (gettid() == pcam->loop_id)
    {
      return;
    }

  pthread_mutex_lock(&pcam->mutex);

  if (pcam->loop_id > 0)
    {
      id = pcam->loop_id;
      pcam->loop_id = 0;
    }

  pthread_mutex_unlock(&pcam->mutex);

  if (id > 0)
    {
      pthread_join(id, &value);
    }
}

/****************************************************************************
 * Name: nxcamera_loopthread
 *
 *  This is the thread that streams the video and handles video controls.
 *
 ****************************************************************************/

static void *nxcamera_loopthread(pthread_addr_t pvarg)
{
  FAR struct nxcamera_s   *pcam = (FAR struct nxcamera_s *)pvarg;
  unsigned int            prio;
  ssize_t                 size;
  struct video_msg_s      msg;
  bool                    streaming = true;
  int                     i;
  int                     ret;
  struct v4l2_buffer      buf;
  uint32_t                type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  vinfo("Entry\n");
  memset(&buf, 0, sizeof(buf));
  for (i = 0; i < pcam->nbuffers; i++)
    {
      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory = V4L2_MEMORY_MMAP;
      buf.index = i;
      ret = ioctl(pcam->capture_fd, VIDIOC_QBUF, (uintptr_t)&buf);
      if (ret < 0)
        {
          verr("VIDIOC_QBUF failed: %d\n", ret);
          goto err_out;
        }
    }

  /* VIDIOC_STREAMON start stream */

  ret = ioctl(pcam->capture_fd, VIDIOC_STREAMON, (uintptr_t)&type);
  if (ret < 0)
    {
      verr("VIDIOC_STREAMON failed: %d\n", ret);
      goto err_out;
    }
  else
    {
      pcam->loopstate = NXCAMERA_STATE_STREAMING;
    }

  /* Loop until we specifically break. streaming == true means that we are
   * still looping waiting for the stream to complete.  All of the data
   * may have been sent, but the stream is not complete until we get
   * VIDEO_MSG_STOP message
   *
   * The normal protocol for looping errors detected by the video driver
   * is as follows:
   *
   * (1) The video driver will indicated the error by returning a negated
   *     error value when the next buffer is enqueued.  The upper level
   *     then knows that this buffer was not queued.
   * (2) The video driver must return all queued buffers using the
   *     VIDEO_MSG_DEQUEUE message.
   */

  while (streaming)
    {
      size = mq_receive(pcam->mq, (FAR char *)&msg, sizeof(msg), &prio);

      /* Validate a message was received */

      if (size == sizeof(msg))
        {
          /* Perform operation based on message id */

          vinfo("message received size %zd id%d\n", size, msg.msg_id);
          switch (msg.msg_id)
            {
              /* Someone wants to stop the stream. */

              case VIDEO_MSG_STOP:

                /* Send a stop message to the device */

                vinfo("Stopping looping\n");
                ioctl(pcam->capture_fd, VIDIOC_STREAMOFF, (uintptr_t)&type);
                streaming = false;
                goto err_out;

              /* Unknown / unsupported message ID */

              default:
                break;
            }
        }

      ret = ioctl(pcam->capture_fd, VIDIOC_DQBUF, (uintptr_t)&buf);
      if (ret < 0)
        {
          verr("Fail DQBUF %d\n", errno);
          goto err_out;
        }

      ret = show_image(pcam, &buf);
      if (ret < 0)
        {
          verr("Fail to show image %d\n", -ret);
          goto err_out;
        }

      ret = ioctl(pcam->capture_fd, VIDIOC_QBUF, (uintptr_t)&buf);
      if (ret < 0)
        {
          verr("Fail QBUF %d\n", errno);
          goto err_out;
        }
    }

  /* Release our video buffers and unregister / release the device */

err_out:
  vinfo("Clean-up and exit\n");

  /* Cleanup */

  pthread_mutex_lock(&pcam->mutex);  /* Lock the mutex */

  close(pcam->display_fd);           /* Close the display device */
  close(pcam->capture_fd);           /* Close the capture device */
  pcam->display_fd = -1;             /* Mark display device as closed */
  pcam->capture_fd = -1;             /* Mark capture device as closed */
  mq_close(pcam->mq);                /* Close the message queue */
  mq_unlink(pcam->mqname);           /* Unlink the message queue */
  pcam->loopstate = NXCAMERA_STATE_IDLE;
  for (i = 0; i < pcam->nbuffers; i++)
    {
      munmap(pcam->bufs[i], pcam->buf_sizes[i]);
    }

  free(pcam->bufs);
  free(pcam->buf_sizes);
  pthread_mutex_unlock(&pcam->mutex);     /* Unlock the mutex */

  vinfo("Exit\n");

  return NULL;
}

/****************************************************************************
 * Public Functions
 ****************************************************************************/

/****************************************************************************
 * Name: nxcamera_setdevice
 *
 *   nxcamera_setdevice() sets the preferred video device to use with the
 *   provided nxcamera context.
 *
 ****************************************************************************/

int nxcamera_setdevice(FAR struct nxcamera_s *pcam,
                       FAR const char *device)
{
  int                    temp_fd;
  struct v4l2_capability caps;

  DEBUGASSERT(pcam != NULL);
  DEBUGASSERT(device != NULL);

  /* Try to open the device */

  temp_fd = open(device, O_RDWR);
  if (temp_fd == -1)
    {
      /* Error opening the device */

      return -errno;
    }

  /* Validate it's a video device by issuing an VIDIOC_QUERYCAP ioctl */

  if (ioctl(temp_fd, VIDIOC_QUERYCAP, (uintptr_t)&caps) != OK)
    {
      /* Not a video device! */

      close(temp_fd);
      return -errno;
    }

  /* Close the file */

  close(temp_fd);

  /* Save the path of the preferred device */

  if ((caps.capabilities & V4L2_CAP_VIDEO_CAPTURE) == 0)
    {
      return -ENODEV;
    }

  strlcpy(pcam->capturedev, device, sizeof(pcam->capturedev));
  return OK;
}

/****************************************************************************
 * Name: nxcamera_setfb
 *
 *   nxcamera_setfb() sets the output framebuffer device to use with the
 *   provided nxcamera context.
 *
 ****************************************************************************/

int nxcamera_setfb(FAR struct nxcamera_s *pcam, FAR const char *device)
{
  int                   temp_fd;
  struct fb_videoinfo_s vinfo;

  DEBUGASSERT(pcam != NULL);
  DEBUGASSERT(device != NULL);

  /* Try to open the device */

  temp_fd = open(device, O_RDWR);
  if (temp_fd == -1)
    {
      /* Error opening the device */

      return -errno;
    }

  /* Validate it's a fb device by issuing an FBIOGET_VIDEOINFO ioctl */

  if (ioctl(temp_fd, FBIOGET_VIDEOINFO, (uintptr_t)&vinfo) != OK)
    {
      /* Not an Video device! */

      close(temp_fd);
      return -errno;
    }

  /* Close the file */

  close(temp_fd);

  if (vinfo.nplanes == 0)
    {
      return -ENODEV;
    }

  /* Save the path of the framebuffer device */

  strlcpy(pcam->displaydev, device, sizeof(pcam->displaydev));
  return OK;
}

/****************************************************************************
 * Name: nxcamera_setfile
 *
 *   nxcamera_setfile() sets the output file path to use with the provided
 *   nxcamera context.
 *
 ****************************************************************************/

int nxcamera_setfile(FAR struct nxcamera_s *pcam, FAR const char *pfile,
                     bool isimage)
{
  int temp_fd;

  DEBUGASSERT(pcam != NULL);
  DEBUGASSERT(pfile != NULL);

  /* Try to open the file */

  temp_fd = open(pfile, O_CREAT | O_RDWR);
  if (temp_fd == -1)
    {
      /* Error opening the file */

      return -errno;
    }

  /* Close the file */

  close(temp_fd);

  /* Save the path of the output file */

  if (isimage)
    {
      strlcpy(pcam->imagepath, pfile, sizeof(pcam->imagepath));
    }
  else
    {
      strlcpy(pcam->videopath, pfile, sizeof(pcam->videopath));
    }

  return OK;
}

/****************************************************************************
 * Name: nxcamera_stop
 *
 *   nxcamera_stop() stops the current playback to loop and closes the
 *   file and the associated device.
 *
 * Input:
 *   pcam    Pointer to the initialized looper context
 *
 ****************************************************************************/

int nxcamera_stop(FAR struct nxcamera_s *pcam)
{
  struct video_msg_s term_msg;

  DEBUGASSERT(pcam != NULL);

  /* Validate we are not in IDLE state */

  pthread_mutex_lock(&pcam->mutex);          /* Lock the mutex */
  if (pcam->loopstate == NXCAMERA_STATE_IDLE)
    {
      pthread_mutex_unlock(&pcam->mutex);    /* Unlock the mutex */
      return OK;
    }

  /* Notify the stream thread that it needs to cancel the stream */

  term_msg.msg_id = VIDEO_MSG_STOP;
  term_msg.u.data = 0;
  mq_send(pcam->mq, (FAR const char *)&term_msg, sizeof(term_msg),
          CONFIG_NXCAMERA_MSG_PRIO);

  pthread_mutex_unlock(&pcam->mutex);

  /* Join the thread.  The thread will do all the cleanup. */

  nxcamera_jointhread(pcam);

  return OK;
}

/****************************************************************************
 * Name: nxcamera_stream
 *
 *   nxcamera_stream() tries to capture and then display the raw data using
 *   the Video system. If a capture device is specified, it will try to use
 *   that device.
 *
 * Input:
 *   pcam    Pointer to the initialized Looper context
 *   width      Capture frame width
 *   height     Capture frame height
 *   framerate  Capture frame rate
 *   format     Capture frame pixel format
 *
 * Returns:
 *   OK         Video is being looped
 *   -EBUSY     Capture device is busy
 *   -ENOSYS    No supported video format found
 *   -ENODEV    No video capture or framebuffer device suitable
 *
 ****************************************************************************/

int nxcamera_stream(FAR struct nxcamera_s *pcam,
                    uint16_t width, uint16_t height,
                    uint32_t framerate, uint32_t format)
{
  struct mq_attr             attr;
  struct sched_param         sparam;
  pthread_attr_t             tattr;
  int                        ret;
  int                        i;
  struct v4l2_buffer         buf;
  struct v4l2_requestbuffers req;
  struct v4l2_streamparm     parm;

  DEBUGASSERT(pcam != NULL);

  pthread_mutex_lock(&pcam->mutex);          /* Lock the mutex */
  if (pcam->loopstate != NXCAMERA_STATE_IDLE)
    {
      pthread_mutex_unlock(&pcam->mutex);    /* Unlock the mutex */
      return -EBUSY;
    }

  pthread_mutex_unlock(&pcam->mutex);    /* Unlock the mutex */

  vinfo("==============================\n");
  vinfo("streaming video\n");
  vinfo("==============================\n");

  /* Try to open the device */

  ret = nxcamera_opendevice(pcam);
  if (ret < 0)
    {
      /* Error opening the device */

      verr("ERROR: nxcamera_opendevice failed: %d\n", ret);
      return ret;
    }

  /* VIDIOC_S_FMT set format */

  pcam->fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  pcam->fmt.fmt.pix.width       = width;
  pcam->fmt.fmt.pix.height      = height;
  pcam->fmt.fmt.pix.field       = V4L2_FIELD_ANY;
  pcam->fmt.fmt.pix.pixelformat = format;

  ret = ioctl(pcam->capture_fd, VIDIOC_S_FMT, (uintptr_t)&pcam->fmt);
  if (ret < 0)
    {
      ret = -errno;
      verr("VIDIOC_S_FMT failed: %d\n", ret);
      return ret;
    }

  memset(&parm, 0, sizeof(parm));
  parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  parm.parm.capture.timeperframe.denominator = framerate;
  parm.parm.capture.timeperframe.numerator = 1;
  ret = ioctl(pcam->capture_fd, VIDIOC_S_PARM, (uintptr_t)&parm);
  if (ret < 0)
    {
      ret = -errno;
      verr("VIDIOC_S_PARM failed: %d\n", ret);
      return ret;
    }

  /* VIDIOC_REQBUFS initiate user pointer I/O */

  memset(&req, 0, sizeof(req));
  req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory = V4L2_MEMORY_MMAP;
  req.count  = CONFIG_VIDEO_REQBUFS_COUNT_MAX;

  ret = ioctl(pcam->capture_fd, VIDIOC_REQBUFS, (uintptr_t)&req);
  if (ret < 0)
    {
      ret = -errno;
      verr("VIDIOC_REQBUFS failed: %d\n", ret);
      return ret;
    }

  if (req.count < 2)
    {
      verr("VIDIOC_REQBUFS failed: not enough buffers\n");
      return -ENOMEM;
    }

  pcam->nbuffers  = req.count;
  pcam->bufs      = calloc(req.count, sizeof(*pcam->bufs));
  pcam->buf_sizes = calloc(req.count, sizeof(*pcam->buf_sizes));
  if (pcam->bufs == NULL || pcam->buf_sizes == NULL)
    {
      verr("Cannot allocate buffer pointers\n");
      ret = -ENOMEM;
      goto err_out;
    }

  /* VIDIOC_QBUF enqueue buffer */

  for (i = 0; i < req.count; i++)
    {
      memset(&buf, 0, sizeof(buf));
      buf.type   = req.type;
      buf.memory = V4L2_MEMORY_MMAP;
      buf.index  = i;
      ret = ioctl(pcam->capture_fd, VIDIOC_QUERYBUF, (uintptr_t)&buf);
      if (ret < 0)
        {
          ret = -errno;
          verr("VIDIOC_QUERYBUF failed: %d\n", ret);
          goto err_out;
        }

      pcam->bufs[i] = mmap(NULL, buf.length,
                           PROT_READ | PROT_WRITE, MAP_SHARED,
                           pcam->capture_fd, buf.m.offset);
      if (pcam->bufs[i] == MAP_FAILED)
        {
          ret = -errno;
          verr("MMAP failed\n");
          goto err_out;
        }

      pcam->buf_sizes[i] = buf.length;
    }

  /* Create a message queue for the loopthread */

  memset(&attr, 0, sizeof(attr));
  attr.mq_maxmsg  = 8;
  attr.mq_msgsize = sizeof(struct video_msg_s);

  snprintf(pcam->mqname, sizeof(pcam->mqname), "/tmp/%lx",
           (unsigned long)((uintptr_t)pcam) & 0xffffffff);

  pcam->mq = mq_open(pcam->mqname, O_RDWR | O_CREAT | O_NONBLOCK, 0644,
                     &attr);
  if (pcam->mq == (mqd_t)-1)
    {
      /* Unable to open message queue! */

      ret = -errno;
      verr("ERROR: mq_open failed: %d\n", ret);
      goto err_out;
    }

  /* Check if there was a previous thread and join it if there was
   * to perform clean-up.
   */

  nxcamera_jointhread(pcam);

  pthread_attr_init(&tattr);
  sparam.sched_priority = sched_get_priority_max(SCHED_FIFO) - 9;
  pthread_attr_setschedparam(&tattr, &sparam);
  pthread_attr_setstacksize(&tattr, CONFIG_NXCAMERA_LOOPTHREAD_STACKSIZE);

  /* Add a reference count to the looper for the thread and start the
   * thread.  We increment for the thread to avoid thread start-up
   * race conditions.
   */

  nxcamera_reference(pcam);
  ret = pthread_create(&pcam->loop_id, &tattr, nxcamera_loopthread,
                       (pthread_addr_t)pcam);
  pthread_attr_destroy(&tattr);
  if (ret != OK)
    {
      ret = -ret;
      verr("ERROR: Failed to create loopthread: %d\n", ret);
      goto err_out;
    }

  /* Name the thread */

  pthread_setname_np(pcam->loop_id, "nxcameraloop");
  return OK;

err_out:
  if (pcam->bufs)
    {
      for (i = 0; i < pcam->nbuffers; i++)
        {
          if (pcam->bufs[i] != NULL && pcam->bufs[i] != MAP_FAILED)
            {
              munmap(pcam->bufs[i], pcam->buf_sizes[i]);
            }
        }

      free(pcam->bufs);
    }

  if (pcam->buf_sizes)
    {
      free(pcam->buf_sizes);
    }

  return ret;
}

/****************************************************************************
 * Name: nxcamera_create
 *
 *   nxcamera_create() allocates and initializes a nxcamera context for
 *   use by further nxcamera operations.  This routine must be called before
 *   to perform the create for proper reference counting.
 *
 * Input Parameters:  None
 *
 * Returned values:
 *   Pointer to the created context or NULL if there was an error.
 *
 ****************************************************************************/

FAR struct nxcamera_s *nxcamera_create(void)
{
  FAR struct nxcamera_s *pcam;
  int err;

  /* Allocate the memory */

  pcam = (FAR struct nxcamera_s *)calloc(1, sizeof(struct nxcamera_s));
  if (pcam == NULL)
    {
      return NULL;
    }

  /* Initialize the context data */

  pcam->loopstate = NXCAMERA_STATE_IDLE;
  pcam->display_fd = -1;
  pcam->capture_fd = -1;
  err = pthread_mutex_init(&pcam->mutex, NULL);
  if (err)
    {
      verr("ERROR: pthread_mutex_init failed: %d\n", err);
      free(pcam);
      pcam = NULL;
    }

  return pcam;
}

/****************************************************************************
 * Name: nxcamera_release
 *
 *   nxcamera_release() reduces the reference count by one and if it
 *   reaches zero, frees the context.
 *
 * Input Parameters:
 *   pcam    Pointer to the NxCamera context
 *
 * Returned values:    None
 *
 ****************************************************************************/

void nxcamera_release(FAR struct nxcamera_s *pcam)
{
  int      refcount;

  /* Check if there was a previous thread and join it if there was */

  nxcamera_jointhread(pcam);

  /* Lock the mutex */

  pthread_mutex_lock(&pcam->mutex);

  /* Reduce the reference count */

  refcount = pcam->crefs--;
  pthread_mutex_unlock(&pcam->mutex);

  /* If the ref count *was* one, then free the context */

  if (refcount == 1)
    {
      pthread_mutex_destroy(&pcam->mutex);
      free(pcam);
    }
}

/****************************************************************************
 * Name: nxcamera_reference
 *
 *   nxcamera_reference() increments the reference count by one.
 *
 * Input Parameters:
 *   pcam    Pointer to the NxCamera context
 *
 * Returned values:    None
 *
 ****************************************************************************/

void nxcamera_reference(FAR struct nxcamera_s *pcam)
{
  /* Lock the mutex */

  pthread_mutex_lock(&pcam->mutex);

  /* Increment the reference count */

  pcam->crefs++;
  pthread_mutex_unlock(&pcam->mutex);
}