BPI-CM9-Linux/aml-comp/hardware/amlogic/arm-isp-6.12/aml_isp5/libaiq/isp5service/video.c

/*
*
* SPDX-License-Identifier: GPL-2.0
*
* Copyright (C) 2020 Amlogic or its affiliates
*
* This program 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; version 2.
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
*/

#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/mman.h>

#include <linux/types.h>
#include <linux/videodev2.h>

#include "camera.h"

struct local_v4l2_fmtdesc {
	__u32		    index;             /* Format number      */
	__u32		    type;              /* enum v4l2_buf_type */
	__u32               flags;
	__u8		    description[32];   /* Description string */
	__u32		    pixelformat;       /* Format fourcc      */
	__u32		    mbus_code;		/* Media bus code    */
	__u32		    reserved[3];
};

int video_dev_open(struct video_dev *vdev)
{
	int fd = -1;

	fd = open(vdev->devname, O_RDWR);
	if (fd < 0) {
		printf("Failed to open %s(%s): %s\n",
		       vdev->entity.name, vdev->devname, strerror(errno));
		return fd;
	}

	vdev->fd = fd;

	return 0;
}

int video_dev_enum_format(struct video_dev *vdev, int index, struct format_info *info)
{
	int ret;
	struct local_v4l2_fmtdesc fdesc = { 0 };

	fdesc.index = index;
	fdesc.type = vdev->buf_type;
	fdesc.mbus_code = info->code;

	ret = ioctl(vdev->fd, VIDIOC_ENUM_FMT, &fdesc);
	if (ret) {
		printf("%s: Failed to enum video format: %s\n",
		       vdev->entity.name, strerror(errno));
		return ret;
	}

	info->fmt = fdesc.pixelformat;
	info->code = fdesc.mbus_code;

	return 0;
}

int video_dev_set_format(struct video_dev *vdev)
{
	int ret = 0;
	struct v4l2_format vfmt;
	struct v4l2_pix_format *pix = &vfmt.fmt.pix;
	struct v4l2_meta_format *meta = &vfmt.fmt.meta;
	struct format_info *fmt = &vdev->format;

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

	vfmt.type = vdev->buf_type;

	pix->width = fmt->width;
	pix->height = fmt->height;
	pix->pixelformat = fmt->fmt;
	pix->field = V4L2_FIELD_NONE;
	ret = ioctl(vdev->fd, VIDIOC_S_FMT, &vfmt);
	if (ret) {
		printf("%s: Failed to set video format: %s\n",
		       vdev->entity.name, strerror(errno));
		return ret;
	}

	if (vfmt.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE ||
	    vfmt.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
		printf("Video %s: %u-%u-0x%x\n", vdev->entity.name,
		       pix->width, pix->height, pix->pixelformat);
	else
		printf("Video %s: 0x%x-%u\n", vdev->entity.name,
		       meta->dataformat, meta->buffersize);

	return 0;
}

int video_dev_set_selection(struct video_dev *vdev)
{
	int ret = 0;
	struct v4l2_selection sel;
	struct rectangle_t *rect = &vdev->rect;

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

	sel.type = vdev->buf_type;
	sel.target = V4L2_SEL_TGT_CROP;
	sel.flags = 0;

	sel.r.left = rect->left;
	sel.r.top = rect->top;
	sel.r.width = rect->width;
	sel.r.height = rect->height;

	ret = ioctl(vdev->fd, VIDIOC_S_SELECTION, &sel);
	if (ret)
		printf("%s: Failed to set selection: %s\n", vdev->entity.name, strerror(errno));

	return ret;
}

int video_dev_splane_alloc_buffs(struct video_dev *vdev, int count)
{
	int i;
	int ret = 0;
	struct v4l2_buffer vbuff;
	struct v4l2_requestbuffers rb;

	if (count > ARRAY_SIZE(vdev->buffers)) {
		printf("Error input buffer count\n");
		return -EINVAL;
	}

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

	rb.type = vdev->buf_type;
	rb.count = count;
	rb.memory = vdev->mem_type;
	ret = ioctl(vdev->fd, VIDIOC_REQBUFS, &rb);
	if (ret) {
		printf("Failed to reqbuff: %s\n", vdev->entity.name);
		return ret;
	}

	for (i = 0; i < count; i++) {
		memset(&vbuff, 0, sizeof(vbuff));

		vbuff.index = i;
		vbuff.type = vdev->buf_type;
		vbuff.memory = vdev->mem_type;
		ret = ioctl(vdev->fd, VIDIOC_QUERYBUF, &vbuff);
		if (ret) {
			printf("%s: Failed to querybuf: %s\n", vdev->entity.name, strerror(errno));
			break;
		}

		vdev->buffers[i].idx = vbuff.index;
		vdev->buffers[i].nplane = 1;
		vdev->buffers[i].planes[0].size = vbuff.length;
		vdev->buffers[i].used = 1;
	}

	return ret;
}

int video_dev_mplane_alloc_buffs(struct video_dev *vdev, int count)
{
	int i;
	int ret = 0;
	struct v4l2_buffer vbuff;
	struct v4l2_requestbuffers rb;
	struct v4l2_plane planes[2] = { 0 };

	if (count > ARRAY_SIZE(vdev->buffers)) {
		printf("Error input buffer count\n");
		return -EINVAL;
	}

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

	rb.type = vdev->buf_type;
	rb.count = count;
	rb.memory = vdev->mem_type;
	ret = ioctl(vdev->fd, VIDIOC_REQBUFS, &rb);
	if (ret) {
		printf("%s: Failed to reqbuff: %s\n", vdev->entity.name, strerror(errno));
		return ret;
	}

	for (i = 0; i < count; i++) {
		memset(&vbuff, 0, sizeof(vbuff));

		vbuff.index = i;
		vbuff.type = vdev->buf_type;
		vbuff.memory = vdev->mem_type;
		vbuff.length = ARRAY_SIZE(planes);
		vbuff.m.planes = planes;
		ret = ioctl(vdev->fd, VIDIOC_QUERYBUF, &vbuff);
		if (ret) {
			printf("%s: Failed to querybuf: %s\n", vdev->entity.name, strerror(errno));
			break;
		}

		vdev->buffers[i].idx = vbuff.index;
		vdev->buffers[i].nplane = vbuff.length;
		vdev->buffers[i].planes[0].size = planes[0].length;
		vdev->buffers[i].planes[1].size = planes[1].length;
		vdev->buffers[i].used = 1;
	}

	return ret;
}


int video_dev_export_buffs(struct video_dev *vdev)
{
	int i, j;
	int ret = 0;
	struct v4l2_exportbuffer expbuf;

	for (i = 0; i < ARRAY_SIZE(vdev->buffers); i++) {
		if (!vdev->buffers[i].used)
			continue;

		for (j = 0; j < vdev->buffers[i].nplane; j++) {
			memset(&expbuf, 0, sizeof(expbuf));

			expbuf.type = vdev->buf_type;
			expbuf.index = vdev->buffers[i].idx;
			expbuf.flags = O_RDWR;
			expbuf.plane = j;
			ret = ioctl(vdev->fd, VIDIOC_EXPBUF, &expbuf);
			if (ret) {
				printf("%s: Failed to export buff: %s\n", vdev->entity.name, strerror(ret));
				break;
			}

			vdev->buffers[i].planes[j].fd = expbuf.fd;
		}
	}

	return ret;
}

int video_dev_mmap_buffs(struct video_dev *vdev)
{
	int i, j;
	void *mem = NULL;
	struct plane_t *plane = NULL;

	for (i = 0; i < ARRAY_SIZE(vdev->buffers); i++) {
		if (!vdev->buffers[i].used)
			continue;

		for (j = 0; j < vdev->buffers[i].nplane; j++) {
			plane = &vdev->buffers[i].planes[j];
			mem = mmap(0, plane->size, PROT_READ | PROT_WRITE, MAP_SHARED, plane->fd, 0);
			if (mem == (void *)-1) {
				printf("%s: Error to mmap buffers: %s\n", vdev->entity.name, strerror(errno));
				break;
			}

			plane->data = mem;
		}
	}

	return -EINVAL;
}

int video_dev_stream_on(struct video_dev *vdev)
{
	int ret = 0;
	int type = vdev->buf_type;

	ret = ioctl(vdev->fd, VIDIOC_STREAMON, &type);
	if (ret)
		printf("Failed to stream on: %s\n", vdev->entity.name);

	return ret;
}

int video_dev_queue_buff(struct video_dev *vdev, struct buffer_t *buff)
{
	int ret = 0;
	struct v4l2_buffer v4l2_buff;
	struct v4l2_plane planes[2] = { 0 };

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

	v4l2_buff.index = buff->idx;
	v4l2_buff.type = vdev->buf_type;
	v4l2_buff.memory = vdev->mem_type;
	v4l2_buff.field = V4L2_FIELD_NONE;

	if (v4l2_buff.type == V4L2_BUF_TYPE_META_OUTPUT) {
		v4l2_buff.length = buff->planes[0].size;
		v4l2_buff.bytesused = buff->planes[0].bytesused;
	} else if (v4l2_buff.type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
		planes[0].m.fd = buff->planes[0].fd;
		planes[0].length = buff->planes[0].size;
		planes[0].bytesused = buff->planes[0].bytesused;
		planes[1].m.fd = buff->planes[1].fd;
		planes[1].length = buff->planes[1].size;
		planes[1].bytesused = buff->planes[1].bytesused;
		v4l2_buff.length = buff->nplane;
		v4l2_buff.m.planes = planes;
	} else {
		v4l2_buff.length = buff->nplane;
		v4l2_buff.m.planes = planes;
	}

	ret = ioctl(vdev->fd, VIDIOC_QBUF, &v4l2_buff);
	if (ret)
		printf("%s: Failed to QBUF: %s\n", vdev->entity.name, strerror(errno));

	return ret;
}

int video_dev_dequeue_buff(struct video_dev *vdev, struct buffer_t *buff)
{
	int ret = 0;
	struct v4l2_buffer v4l2_buff;
	struct v4l2_plane planes[2] = { 0 };

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

	v4l2_buff.type = vdev->buf_type;
	v4l2_buff.memory = vdev->mem_type;

	if (v4l2_buff.type == V4L2_BUF_TYPE_META_OUTPUT) {
		v4l2_buff.length = 0;
		v4l2_buff.bytesused = 0;
	} else {
		v4l2_buff.length = ARRAY_SIZE(planes);
		v4l2_buff.m.planes = planes;
	}

	ret = ioctl(vdev->fd, VIDIOC_DQBUF, &v4l2_buff);
	if (ret) {
		printf("%s: Failed to DQBUF: %s\n", vdev->entity.name, strerror(errno));
		return ret;
	}

	buff->idx = v4l2_buff.index;
	buff->sequence = v4l2_buff.sequence;

	return ret;
}

int video_dev_query_ctrls(struct video_dev *vdev)
{
	int i;
	int ret = 0;
	struct v4l2_query_ext_ctrl *ctrl = NULL;

	vdev->qctrls.count = 0;

	for (i = 0; i < ARRAY_SIZE(vdev->qctrls.qctrls); i++) {
		ctrl = &vdev->qctrls.qctrls[i];

		ctrl->id |= V4L2_CTRL_FLAG_NEXT_CTRL |
			V4L2_CTRL_FLAG_NEXT_COMPOUND;
		ret = ioctl(vdev->fd, VIDIOC_QUERY_EXT_CTRL, ctrl);
		if (ret)
			break;

		if (ctrl->type == V4L2_CTRL_TYPE_CTRL_CLASS ||
			ctrl->flags & V4L2_CTRL_FLAG_DISABLED)
			continue;

		switch (ctrl->type) {
			case V4L2_CTRL_TYPE_INTEGER:
			case V4L2_CTRL_TYPE_BOOLEAN:
			case V4L2_CTRL_TYPE_MENU:
			case V4L2_CTRL_TYPE_BUTTON:
			case V4L2_CTRL_TYPE_INTEGER64:
			case V4L2_CTRL_TYPE_BITMASK:
			case V4L2_CTRL_TYPE_INTEGER_MENU:
			case V4L2_CTRL_TYPE_U8:
				vdev->qctrls.count++;
			break;
			default:
				i--;
				printf("Error to support this ctrl type: 0x%x\n", ctrl->type);
			continue;
		}
	}

	return 0;
}

int video_dev_set_ctrls(struct video_dev *vdev, void *ctrls, int count)
{
	int ret = 0;
	struct v4l2_ext_controls ext_ctrls;

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

	ext_ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
	ext_ctrls.controls = ctrls;
	ext_ctrls.count = count;

	ret = ioctl(vdev->fd, VIDIOC_S_EXT_CTRLS, &ext_ctrls);
	if (ret)
		printf("Failed to set  extern controls: %s\n", vdev->entity.name);

	return ret;
}

int video_dev_get_ctrls(struct video_dev *vdev, void *ctrls, int count)
{
	int ret = 0;
	struct v4l2_ext_controls ext_ctrls;

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

	ext_ctrls.which = V4L2_CTRL_WHICH_CUR_VAL;
	ext_ctrls.controls = ctrls;
	ext_ctrls.count = count;

	ret = ioctl(vdev->fd, VIDIOC_G_EXT_CTRLS, &ext_ctrls);
	if (ret)
		printf("Failed to get extern controls: %s\n", vdev->entity.name);

	return ret;
}

int video_dev_subscribe_event(struct video_dev *vdev, void *subscribe)
{
	int ret = 0;
	struct v4l2_event_subscription *event = subscribe;

	ret = ioctl(vdev->fd, VIDIOC_SUBSCRIBE_EVENT, event);
	if (ret)
		printf("Failed to subscribe event: %s\n", vdev->entity.name);

	return ret;
}

int video_dev_unsubscribe_event(struct video_dev *vdev, void *subscribe)
{
	int ret = 0;
	struct v4l2_event_subscription *event = subscribe;

	ret = ioctl(vdev->fd, VIDIOC_UNSUBSCRIBE_EVENT, event);
	if (ret)
		printf("Failed to unsubscribe event: %s\n", vdev->entity.name);

	return ret;
}


int video_dev_dequeue_event(struct video_dev *vdev, struct v4l2_event *event)
{
	int ret = 0;

	ret = ioctl(vdev->fd, VIDIOC_DQEVENT, event);
	if (ret)
		printf("Failed to dqevent: %s\n", vdev->entity.name);

	return ret;
}

int video_dev_subscribe_fsync(struct video_dev *vdev)
{
	struct v4l2_event_subscription event;

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

	event.type = V4L2_EVENT_FRAME_SYNC;

	return video_dev_subscribe_event(vdev, &event);
}

int video_dev_get_fsync(struct video_dev *vdev)
{
	int ret = 0;
	struct v4l2_event event;

	ret = video_dev_dequeue_event(vdev, &event);
	if (ret)
		goto err_return;

	if (event.type != V4L2_EVENT_FRAME_SYNC)
		ret = -EINVAL;

err_return:
	return ret;
}

int video_dev_unsubscribe_fsync(struct video_dev *vdev)
{
	struct v4l2_event_subscription event;

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

	event.type = V4L2_EVENT_FRAME_SYNC;

	return video_dev_unsubscribe_event(vdev, &event);
}

void video_dev_stream_off(struct video_dev *vdev)
{
	int ret;
	int type = vdev->buf_type;

	ret = ioctl(vdev->fd, VIDIOC_STREAMOFF, &type);
	if (ret)
		printf("Failed to stream of: %s\n", vdev->entity.name);
}

void video_dev_unmap_buffs(struct video_dev *vdev)
{
	int i, j;
	struct plane_t *plane = NULL;

	for (i = 0; i < ARRAY_SIZE(vdev->buffers); i++) {
		if (!vdev->buffers[i].used)
			continue;

		for (j = 0; j < vdev->buffers[i].nplane; j++) {
			plane = &vdev->buffers[i].planes[j];

			munmap(plane->data, plane->size);
			plane->data = NULL;
		}
	}
}

void video_dev_close_buffs(struct video_dev *vdev)
{
	int i, j;
	struct plane_t *plane;

	for (i = 0; i < ARRAY_SIZE(vdev->buffers); i++) {
		if (!vdev->buffers[i].used)
			continue;

		for (j = 0; j < vdev->buffers[i].nplane; j++) {
			plane = &vdev->buffers[i].planes[j];

			close(plane->fd);
			plane->fd = -1;
		}
	}
}

void video_dev_release_buffs(struct video_dev *vdev)
{
	struct v4l2_requestbuffers rb;

	rb.count = 0;
	rb.type = vdev->buf_type;
	rb.memory = vdev->mem_type;

	ioctl(vdev->fd, VIDIOC_REQBUFS, &rb);
}

void video_dev_close(struct video_dev *vdev)
{
	close(vdev->fd);
}