sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
7faebbd425
nuttx-apps/system/nxcamera/nxcamera.c
Xiang Xiao a29d9ea9da fsutils/examples: Include unistd.h explicitly 
to get the prototypes or macros are defined in it

Signed-off-by: Xiang Xiao <xiaoxiang@xiaomi.com>
2023-02-05 08:46:59 +02:00

864 lines
23 KiB
C
Raw Blame History

/****************************************************************************
* 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 <unistd.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);
}
Reference in New Issue View Git Blame Copy Permalink