/**************************************************************************** * 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 #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef CONFIG_LIBYUV # include #endif /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ #define NXCAMERA_STATE_IDLE 0 #define NXCAMERA_STATE_STREAMING 1 #define NXCAMERA_STATE_LOOPING 2 #define NXCAMERA_STATE_PAUSED 3 /**************************************************************************** * Private Functions ****************************************************************************/ /**************************************************************************** * pan_display ****************************************************************************/ static void pan_display(int fb_device, FAR struct fb_planeinfo_s *plane_info) { struct pollfd pfd; int ret; pfd.fd = fb_device; pfd.events = POLLOUT; ret = poll(&pfd, 1, 0); if (ret > 0) { ioctl(fb_device, FBIOPAN_DISPLAY, plane_info); } } static int show_image(FAR struct nxcamera_s *pcam, FAR v4l2_buffer_t *buf) { #ifdef CONFIG_LIBYUV if (pcam->display_vinfo.fmt == FB_FMT_RGB32) { return ConvertToARGB(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 if (pcam->display_vinfo.fmt == FB_FMT_RGB16_565) { if (pcam->fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) { return ConvertFromI420(pcam->bufs[buf->index], pcam->fmt.fmt.pix.width, &pcam->bufs[buf->index][ pcam->fmt.fmt.pix.width * pcam->fmt.fmt.pix.height], pcam->fmt.fmt.pix.width / 2, &pcam->bufs[buf->index][ pcam->fmt.fmt.pix.width * pcam->fmt.fmt.pix.height * 5 / 4], pcam->fmt.fmt.pix.width / 2, pcam->display_pinfo.fbmem, pcam->display_pinfo.stride, pcam->fmt.fmt.pix.width, pcam->fmt.fmt.pix.height, V4L2_PIX_FMT_RGB565); } else { int ret; FAR uint8_t *dst = malloc(pcam->fmt.fmt.pix.width * pcam->fmt.fmt.pix.height * 3 / 2); if (!dst) { return -ENOMEM; } ret = ConvertToI420(pcam->bufs[buf->index], pcam->buf_sizes[buf->index], dst, pcam->fmt.fmt.pix.width, &dst[pcam->fmt.fmt.pix.width * pcam->fmt.fmt.pix.height], pcam->fmt.fmt.pix.width / 2, &dst[pcam->fmt.fmt.pix.width * pcam->fmt.fmt.pix.height * 5 / 4], pcam->fmt.fmt.pix.width / 2, 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); if (ret < 0) { goto err; } ret = ConvertFromI420(dst, pcam->fmt.fmt.pix.width, &dst[pcam->fmt.fmt.pix.width * pcam->fmt.fmt.pix.height], pcam->fmt.fmt.pix.width / 2, &dst[pcam->fmt.fmt.pix.width * pcam->fmt.fmt.pix.height * 5 / 4], pcam->fmt.fmt.pix.width / 2, pcam->display_pinfo.fbmem, pcam->display_pinfo.stride, pcam->fmt.fmt.pix.width, pcam->fmt.fmt.pix.height, V4L2_PIX_FMT_RGB565); if (ret < 0) { goto err; } err: free(dst); return ret; } } return 0; #else FAR uint32_t *pbuf = (FAR uint32_t *)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; } if (pcam->display_pinfo.yres_virtual > pcam->display_vinfo.yres) { pan_display(pcam->display_fd, &pcam->display_pinfo); } 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; 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)&pcam->display_vinfo) != OK) { /* Not an Video device! */ close(temp_fd); return -errno; } /* Close the file */ close(temp_fd); if (pcam->display_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); }