/*
*
* 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 <poll.h>
#include <stdio.h>
#include <malloc.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>

#include "camera.h"
#include "video.h"
#include "subdev.h"
#include "handler.h"

#define POLL_TIMEOUT (500)

static int camera_video_buff_ready(struct video_dev *vdev)
{
	int ret;
	struct buffer_t buff = { 0 };

	if (!vdev->used)
		return 0;

	ret = video_dev_dequeue_buff(vdev, &buff);
	if (ret)
		return ret;

	vdev->buffers[buff.idx].sequence = buff.sequence;

	video_dev_buff_ready(vdev, &vdev->buffers[buff.idx]);

	return 0;
}

static int camera_video_status_process(struct video_dev *vdev)
{
	int ret;
	struct v4l2_event event;

	if (!vdev->used)
		return 0;

	ret = video_dev_dequeue_event(vdev, &event);
	if (ret) {
		printf("Failed to dequeue event for %s: %s\n", vdev->entity.name, strerror(errno));
		return ret;
	}

	switch (event.type) {
	case AML_ISP_V4L2_EVENT_STREAM_START: {
		pthread_mutex_lock(&vdev->lock);
		vdev->status = STATUS_ON;
		pthread_mutex_unlock(&vdev->lock);
		printf("%s stream start\n", vdev->entity.name);
		break;
	}
	case AML_ISP_V4L2_EVENT_STREAM_STOP: {
		pthread_mutex_lock(&vdev->lock);
		vdev->status = STATUS_OFF;
		pthread_mutex_unlock(&vdev->lock);
		printf("%s stream stop\n", vdev->entity.name);
		break;
	}
	default:
		printf("Unknown event type %u for %s\n", event.type, vdev->entity.name);
		break;
	}
	return 0;
}

static int camera_video_event_process(struct camera_dev *cam, struct pollfd *pfds, int rcnt)
{
	int cnt = 0;
	unsigned int i;
	for (i = VIDEO_STATS; i < VIDEO_MAX; i++) {
		if (cam->status == STATUS_ON && ((pfds[i].revents & POLLIN) || (pfds[i].revents & POLLOUT))) {
			cnt++;
			switch (i) {
			case VIDEO_STATS:
				camera_video_buff_ready(&cam->stats);
				break;
			case VIDEO_IMP:
				camera_video_buff_ready(&cam->imp);
				break;
			case VIDEO_PARAM:
				camera_video_buff_ready(&cam->param);
				break;
			case VIDEO_RAW:
				camera_video_buff_ready(&cam->raw);
				break;
#ifdef CAPTURE_SUPPORT_VIEW
			case VIDEO_VIEW:
				camera_video_buff_ready(&cam->view);
				break;
			case VIDEO_CAP:
				camera_video_buff_ready(&cam->cap);
				break;
			case VIDEO_RECORD:
				camera_video_buff_ready(&cam->record);
				break;
			case VIDEO_DISP:
				camera_video_buff_ready(&cam->disp);
				break;
#endif
			case VIDEO_VC0:
				camera_video_buff_ready(&cam->vc0);
				break;
			case VIDEO_VC1:
				camera_video_buff_ready(&cam->vc1);
				break;
			case VIDEO_VC2:
				camera_video_buff_ready(&cam->vc2);
				break;
			case VIDEO_VC3:
				camera_video_buff_ready(&cam->vc3);
				break;
			default:
				printf("Error input video index\n");
				break;
			}
		} else if (pfds[i].revents & POLLPRI) {
			cnt++;
			switch (i) {
			case VIDEO_VIEW:
				camera_video_status_process(&cam->view);
				break;
			case VIDEO_CAP:
				camera_video_status_process(&cam->cap);
				break;
			case VIDEO_RECORD:
				camera_video_status_process(&cam->record);
				break;
			case VIDEO_DISP:
				camera_video_status_process(&cam->disp);
				break;
			default:
				printf("Error input video index for stream status\n");
				break;
			}
			if (cam->view.status == STATUS_OFF && cam->cap.status == STATUS_OFF && cam->record.status == STATUS_OFF && cam->disp.status == STATUS_OFF && cam->status == STATUS_ON) {
				pthread_mutex_lock(&cam->lock);
				cam->status = STATUS_PAUSE;
				pthread_mutex_unlock(&cam->lock);
				videos_dev_thread_destroy(cam);
				camera_video_stream_off(cam);
				camera_video_release_buffs(cam);
				camera_video_alloc_buffs(cam);
				camera_video_queue_buffs(cam);
				camera_video_stream_on(cam);
				printf("All stream stop, camera pause\n");
			} else if ((cam->view.status == STATUS_ON || cam->cap.status == STATUS_ON || cam->record.status == STATUS_ON || cam->disp.status == STATUS_ON) && cam->status == STATUS_PAUSE) {
				pthread_mutex_lock(&cam->lock);
				cam->status = STATUS_ON;
				pthread_mutex_unlock(&cam->lock);
				camera_video_stream_on_vc(cam);
				videos_dev_thread_create(cam);
			}
		}

		if (cnt == rcnt)
			break;
	}

	return 0;
}

static void *camera_video_event_thread(void *param)
{
	int ret = 0;
	struct pollfd pfds[VIDEO_MAX];
	struct camera_dev *cam = param;

	struct v4l2_event_subscription sub;

	// struct video_dev *video_devs[] = {&cam->view, &cam->cap, &cam->record, &cam->disp};
	struct video_dev *video_devs[] = {&cam->view};
	printf("Camera %s subscribe start\n", cam->name);
	for (int i = 0; i < ARRAY_SIZE(video_devs); i++) {
		memset(&sub, 0, sizeof(sub));
		sub.type = AML_ISP_V4L2_EVENT_STREAM_START;
		ret = video_dev_subscribe_event(video_devs[i], &sub);
		if (ret) {
			printf("Error to subscribe start event fd %d error: %s\n", video_devs[i]->fd, strerror(errno));
			return NULL;
		}

		memset(&sub, 0, sizeof(sub));
		sub.type = AML_ISP_V4L2_EVENT_STREAM_STOP;
		ret = video_dev_subscribe_event(video_devs[i], &sub);
		if (ret) {
			printf("Error to subscribe stop event fd %d error: %s\n", video_devs[i]->fd, strerror(errno));
			return NULL;
		}
	}
	printf("Camera %s subscribe events end\n", cam->name);
	pfds[VIDEO_STATS].fd = cam->stats.fd;
	pfds[VIDEO_STATS].events = POLLIN;

	pfds[VIDEO_IMP].fd = cam->imp.fd;
	pfds[VIDEO_IMP].events = POLLOUT;

	pfds[VIDEO_PARAM].fd = cam->param.fd;
	pfds[VIDEO_PARAM].events = POLLOUT;

	pfds[VIDEO_RAW].fd = cam->raw.fd;
	pfds[VIDEO_RAW].events = POLLIN;

	pfds[VIDEO_VIEW].fd = cam->view.fd;
	pfds[VIDEO_CAP].fd = cam->cap.fd;
	pfds[VIDEO_RECORD].fd = cam->record.fd;
	pfds[VIDEO_DISP].fd = cam->disp.fd;

#ifdef CAPTURE_SUPPORT_VIEW
	pfds[VIDEO_VIEW].events = POLLIN;
	pfds[VIDEO_CAP].events = POLLIN;
	pfds[VIDEO_DISP].events = POLLIN;
	pfds[VIDEO_RECORD].events = POLLIN;
#else
	pfds[VIDEO_VIEW].events = POLLPRI;
	pfds[VIDEO_CAP].events = POLLPRI;
	pfds[VIDEO_DISP].events = POLLPRI;
	pfds[VIDEO_RECORD].events = POLLPRI;
#endif

	pfds[VIDEO_VC0].fd = cam->vc0.fd;
	pfds[VIDEO_VC0].events = POLLIN;

	pfds[VIDEO_VC1].fd = cam->vc1.fd;
	pfds[VIDEO_VC1].events = POLLIN;

	pfds[VIDEO_VC2].fd = cam->vc2.fd;
	pfds[VIDEO_VC2].events = POLLIN;

	pfds[VIDEO_VC3].fd = cam->vc3.fd;
	pfds[VIDEO_VC3].events = POLLIN;

	while (cam->status != STATUS_OFF) {
		ret = poll(pfds, ARRAY_SIZE(pfds), POLL_TIMEOUT);
		if (ret <= 0)
			continue;

		camera_video_event_process(cam, pfds, ret);
	}

	return NULL;
}


static int camera_alg_param_init(struct camera_dev *cam, struct buffer_t *buff)
{
	struct format_info fmt;
	int ret;

	ret = sensor_common_config(cam);
	if (ret) {
		printf("Error to config sensor\n");
		return ret;
	}

	ret = sensor_get_format(&cam->sensor, &fmt);
	if (ret) {
		printf("Error to get sensor format\n");
		return ret;
	}

	alg_params_init(&fmt, buff, cam->cam_id);

	printf("Initial algorithm parameters\n");

	return 0;
}

static int camera_video_init_buff_queue(struct video_dev *vdev)
{
	int i;
	int ret = 0;
	struct buffer_t *buff;

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

		if (vdev->buf_type == V4L2_BUF_TYPE_META_OUTPUT ||
		    vdev->buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
			video_dev_buff_ready(vdev, buff);
		} else {
			ret = video_dev_queue_buff(vdev, buff);
			if (ret)
				break;
		}
	}

	return ret;
}

int camera_dev_init(struct camera_dev *cam)
{
	subdev_open(&cam->sensor);
	subdev_open(&cam->csiphy);
	subdev_open(&cam->adap);
	subdev_open(&cam->ipp);
	subdev_open(&cam->isp);
	subdev_open(&cam->view_rsz);
	subdev_open(&cam->cap_rsz);
	subdev_open(&cam->record_rsz);
	subdev_open(&cam->disp_rsz);

	cam->view.status = STATUS_OFF;
	cam->cap.status = STATUS_OFF;
	cam->record.status = STATUS_OFF;
	cam->disp.status = STATUS_OFF;

	if (cam->stats.used) {
		video_dev_open(&cam->stats);
		cam->stats.buf_type = V4L2_BUF_TYPE_META_CAPTURE;
		cam->stats.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->stats.head);
	}

	if (cam->imp.used) {
		video_dev_open(&cam->imp);
		cam->imp.buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
		cam->imp.mem_type = V4L2_MEMORY_DMABUF;
		local_list_init_head(&cam->imp.head);
	}

	if (cam->param.used) {
		video_dev_open(&cam->param);
		cam->param.buf_type = V4L2_BUF_TYPE_META_OUTPUT;
		cam->param.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->param.head);
	}

	if (cam->raw.used) {
		video_dev_open(&cam->raw);
		cam->raw.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		cam->raw.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->raw.head);
	}

	if (cam->view.used) {
		video_dev_open(&cam->view);
#ifdef CAPTURE_SUPPORT_VIEW
		cam->view.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		cam->view.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->view.head);
#endif
	}

	if (cam->cap.used) {
		video_dev_open(&cam->cap);
#ifdef CAPTURE_SUPPORT_VIEW
		cam->cap.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		cam->cap.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->cap.head);
#endif
	}

	if (cam->record.used) {
		video_dev_open(&cam->record);
#ifdef CAPTURE_SUPPORT_VIEW
		cam->record.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		cam->record.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->record.head);
#endif
	}

	if (cam->disp.used) {
		video_dev_open(&cam->disp);
#ifdef CAPTURE_SUPPORT_VIEW
		cam->disp.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		cam->disp.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->disp.head);
#endif
	}

	if (cam->vc0.used) {
		video_dev_open(&cam->vc0);
		cam->vc0.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		cam->vc0.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->vc0.head);
	}

	if (cam->vc1.used) {
		video_dev_open(&cam->vc1);
		cam->vc1.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		cam->vc1.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->vc1.head);
	}

	if (cam->vc2.used) {
		video_dev_open(&cam->vc2);
		cam->vc2.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		cam->vc2.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->vc2.head);
	}

	if (cam->vc3.used) {
		video_dev_open(&cam->vc3);
		cam->vc3.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		cam->vc3.mem_type = V4L2_MEMORY_MMAP;
		local_list_init_head(&cam->vc3.head);
	}

	subdev_enum_mbus_code(&cam->sensor);
	subdev_enum_frame_size(&cam->sensor);

	return 0;
}

int camera_dev_create_links(struct camera_dev *cam)
{
	struct media_link *link = NULL;

	/* ISP link to ipp or import */
	if (cam->raw.used || cam->view.used || cam->cap.used ||
	    cam->record.used || cam->disp.used) {
		if (cam->isp.work_mode == C4_ISP_OFFTILE_MODE ||
		    cam->isp.work_mode == C4_ISP_OFFPOST_MODE)
			link = cam->imp.entity.pads[0].links[0];
		else
			link = cam->ipp.entity.pads[1].links[0];

		media_setup_link(cam, link, 1);
	}

	/* IPP link to adap */
	link = cam->ipp.entity.pads[C4_IPP_PAD_SINK_VIDEO].links[0];
	media_setup_link(cam, link, 1);

	/* adap link to CSI-2 */
	link = cam->adap.entity.pads[0].links[0];
	media_setup_link(cam, link, 1);

	/* CSI-2 link to sensor */
	link = cam->csiphy.entity.pads[0].links[0];
	media_setup_link(cam, link, 1);

	if (cam->param.used) {
		link = cam->isp.entity.pads[C4_ISP_CORE_PAD_SINK_PARAMS].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->stats.used) {
		link = cam->isp.entity.pads[C4_ISP_CORE_PAD_SOURCE_STATS].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->raw.used) {
		link = cam->raw.entity.pads[0].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->view.used) {
		link = cam->view_rsz.entity.pads[0].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->cap.used) {
		link = cam->cap_rsz.entity.pads[0].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->record.used) {
		link = cam->record_rsz.entity.pads[0].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->disp.used) {
		link = cam->disp_rsz.entity.pads[0].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->vc0.used) {
		link = cam->ipp.entity.pads[C4_IPP_PAD_SOURCE_VIDEO_0].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->vc1.used) {
		link = cam->ipp.entity.pads[C4_IPP_PAD_SOURCE_VIDEO_1].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->vc2.used) {
		link = cam->ipp.entity.pads[C4_IPP_PAD_SOURCE_VIDEO_2].links[0];
		media_setup_link(cam, link, 1);
	}

	if (cam->vc3.used) {
		link = cam->ipp.entity.pads[C4_IPP_PAD_SOURCE_VIDEO_3].links[0];
		media_setup_link(cam, link, 1);
	}

	return 0;
}

int camera_subdev_cfg_ctrls(struct camera_dev *cam)
{
	struct v4l2_ext_control ctrls[3];

	ctrls[0].id = V4L2_CID_C4_ISP_WORK_MODE;
	ctrls[0].value = cam->isp.work_mode;
	ctrls[1].id = V4L2_CID_C4_ISP_IMAGE_MODE;
	ctrls[1].value = cam->isp.img_mode;
	subdev_set_ctrls(&cam->isp, &ctrls, 2);

	ctrls[0].id = V4L2_CID_C4_IPP_WORK_MODE;
	ctrls[0].value = cam->ipp.work_mode;
	ctrls[1].id = V4L2_CID_C4_IPP_IMAGE_MODE;
	ctrls[1].value = cam->ipp.img_mode;
	ctrls[2].id = V4L2_CID_C4_IPP_PATTERN_GEN;
	ctrls[2].value = cam->ipp.patgen;
	subdev_set_ctrls(&cam->ipp, &ctrls, 3);

	ctrls[0].id = V4L2_CID_C4_ADAP_IMAGE_MODE;
	ctrls[0].value = cam->adap.img_mode;
	subdev_set_ctrls(&cam->adap, &ctrls, 1);

	ctrls[0].id = V4L2_CID_SENSOR_IMAGE_MODE;
	ctrls[0].value = cam->sensor.img_mode;
	subdev_set_ctrls(&cam->sensor, &ctrls, 1);

	return 0;
}

int camera_subdev_cfg_format(struct camera_dev *cam)
{
	int ret;
	struct format_info sfmt;
	struct v4l2_rect crop_rect;
	struct v4l2_rect scale_rect;

	ret = sensor_get_format(&cam->sensor, &sfmt);
	if (ret) {
		printf("Failed to get sensor format\n");
		return ret;
	}

	subdev_set_format(&cam->sensor, 0, &sfmt);

	ret = subdev_get_selection(&cam->sensor, 0, V4L2_SEL_TGT_CROP, &crop_rect);
	if (ret) {
		printf("Failed to get sensor selection\n");
		return ret;
	}

	printf("Sensor format %u-%u-0x%x\n", sfmt.width, sfmt.height, sfmt.code);
	printf("Sensor selection %d-%d-%u-%u\n", crop_rect.left, crop_rect.top,
		crop_rect.width, crop_rect.height);

	subdev_set_format(&cam->csiphy, 0, &sfmt);
	subdev_set_format(&cam->adap, 0, &sfmt);

	subdev_set_format(&cam->ipp, C4_IPP_PAD_SINK_VIDEO, &sfmt);

	if (cam->vc0.used) {
		subdev_set_format(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_0, &sfmt);
		subdev_set_selection(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_0,
				     V4L2_SEL_TGT_CROP, &crop_rect);
	}

	if (cam->vc1.used) {
		subdev_set_format(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_1, &sfmt);
		subdev_set_selection(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_1,
				     V4L2_SEL_TGT_CROP, &crop_rect);
	}

	if (cam->vc2.used) {
		subdev_set_format(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_2, &sfmt);
		subdev_set_selection(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_2,
				     V4L2_SEL_TGT_CROP, &crop_rect);
	}

	if (cam->vc3.used) {
		subdev_set_format(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_3, &sfmt);
		subdev_set_selection(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_3,
				     V4L2_SEL_TGT_CROP, &crop_rect);
	}

	subdev_set_format(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_ISP, &sfmt);
	subdev_set_selection(&cam->ipp, C4_IPP_PAD_SOURCE_VIDEO_ISP,
			     V4L2_SEL_TGT_CROP, &crop_rect);

	subdev_set_format(&cam->isp, C4_ISP_CORE_PAD_SINK_VIDEO, &sfmt);

	if (cam->raw.used)
		subdev_set_format(&cam->isp, C4_ISP_CORE_PAD_SOURCE_RAW, &sfmt);

	sfmt.code = MEDIA_BUS_FMT_YUV10_1X30;

	if (cam->view.used) {
		subdev_set_format(&cam->isp, C4_ISP_CORE_PAD_SOURCE_VIDEO_0, &sfmt);
		subdev_set_format(&cam->view_rsz, 0, &sfmt);

		crop_rect.left = 0;
		crop_rect.top = 0;
		crop_rect.width = sfmt.width;
		crop_rect.height = sfmt.height;
		subdev_set_selection(&cam->view_rsz, 0, V4L2_SEL_TGT_CROP, &crop_rect);

		scale_rect.left = 0;
		scale_rect.top = 0;
		scale_rect.width = sfmt.width;
		scale_rect.height = sfmt.height;
		subdev_set_selection(&cam->view_rsz, 0, V4L2_SEL_TGT_COMPOSE, &scale_rect);
	}

	if (cam->cap.used) {
		subdev_set_format(&cam->isp, C4_ISP_CORE_PAD_SOURCE_VIDEO_1, &sfmt);
		subdev_set_format(&cam->cap_rsz, 0, &sfmt);

		crop_rect.left = 0;
		crop_rect.top = 0;
		crop_rect.width = sfmt.width;
		crop_rect.height = sfmt.height;
		subdev_set_selection(&cam->cap_rsz, 0, V4L2_SEL_TGT_CROP, &crop_rect);

		scale_rect.left = 0;
		scale_rect.top = 0;
		scale_rect.width = cam->cap.format.width;
		scale_rect.height = cam->cap.format.height;
		subdev_set_selection(&cam->cap_rsz, 0, V4L2_SEL_TGT_COMPOSE, &scale_rect);
	}

	if (cam->record.used) {
		subdev_set_format(&cam->isp, C4_ISP_CORE_PAD_SOURCE_VIDEO_2, &sfmt);
		subdev_set_format(&cam->record_rsz, 0, &sfmt);

		crop_rect.left = 0;
		crop_rect.top = 0;
		crop_rect.width = sfmt.width;
		crop_rect.height = sfmt.height;
		subdev_set_selection(&cam->record_rsz, 0, V4L2_SEL_TGT_CROP, &crop_rect);

		scale_rect.left = 0;
		scale_rect.top = 0;
		scale_rect.width = cam->record.format.width;
		scale_rect.height = cam->record.format.height;
		subdev_set_selection(&cam->record_rsz, 0, V4L2_SEL_TGT_COMPOSE, &scale_rect);
	}

	if (cam->disp.used) {
		subdev_set_format(&cam->isp, C4_ISP_CORE_PAD_SOURCE_VIDEO_3, &sfmt);
		subdev_set_format(&cam->disp_rsz, 0, &sfmt);

		crop_rect.left = 0;
		crop_rect.top = 0;
		crop_rect.width = sfmt.width;
		crop_rect.height = sfmt.height;
		subdev_set_selection(&cam->disp_rsz, 0, V4L2_SEL_TGT_CROP, &crop_rect);

		scale_rect.left = 0;
		scale_rect.top = 0;
		scale_rect.width = cam->disp.format.width;
		scale_rect.height = cam->disp.format.height;
		subdev_set_selection(&cam->disp_rsz, 0, V4L2_SEL_TGT_COMPOSE, &scale_rect);
	}

	return 0;
}

int camera_subdev_query_ctrls(struct camera_dev *cam)
{
	subdev_query_ctrls(&cam->sensor);

	return 0;
}

int camera_video_prep_offtile_format(struct camera_dev *cam)
{
	struct format_info sfmt = { 0 };
	int ret;

	ret = subdev_get_format(&cam->ipp, C4_IPP_PAD_SINK_VIDEO, &sfmt);
	if (ret)
		return ret;

	if (cam->vc0.used) {
		cam->vc0.format = sfmt;
		ret = video_dev_enum_format(&cam->vc0, 0, &cam->vc0.format);
		if (ret)
			return ret;
	}

	if (cam->vc1.used) {
		cam->vc1.format = sfmt;
		ret = video_dev_enum_format(&cam->vc1, 0, &cam->vc1.format);
		if (ret)
			return ret;
	}

	if (cam->imp.used) {
		cam->imp.format = sfmt;
		ret = video_dev_enum_format(&cam->imp, 0, &cam->imp.format);
		if (ret)
			return ret;
	}

	return 0;
}

int camera_video_prep_offpost_format(struct camera_dev *cam)
{
	struct format_info sfmt = { 0 };
	int ret;

	ret = subdev_get_format(&cam->ipp, C4_IPP_PAD_SINK_VIDEO, &sfmt);
	if (ret)
		return ret;

	if (cam->vc0.used) {
		cam->vc0.format = sfmt;
		cam->vc0.format.fmt = V4L2_PIX_FMT_NV12M;
	}

	if (cam->vc1.used) {
		cam->vc1.format = sfmt;
		cam->vc1.format.fmt = V4L2_PIX_FMT_NV12M;
	}

	if (cam->vc2.used) {
		cam->vc2.format = sfmt;
		cam->vc2.format.fmt = V4L2_PIX_FMT_NV12M;
	}

	if (cam->vc3.used) {
		cam->vc3.format = sfmt;
		cam->vc3.format.fmt = V4L2_PIX_FMT_NV12M;
	}

	if (cam->imp.used) {
		cam->imp.format = sfmt;
		cam->imp.format.fmt = V4L2_PIX_FMT_NV12M;
	}

	return 0;
}

int camera_video_cfg_format(struct camera_dev *cam)
{
	if (cam->imp.used)
		video_dev_set_format(&cam->imp);

	if (cam->raw.used)
		video_dev_set_format(&cam->raw);

#ifdef CAPTURE_SUPPORT_VIEW
	if (cam->view.used)
		video_dev_set_format(&cam->view);

	if (cam->cap.used)
		video_dev_set_format(&cam->cap);

	if (cam->record.used)
		video_dev_set_format(&cam->record);

	if (cam->disp.used)
		video_dev_set_format(&cam->disp);
#endif

	if (cam->vc0.used)
		video_dev_set_format(&cam->vc0);

	if (cam->vc1.used)
		video_dev_set_format(&cam->vc1);

	if (cam->vc2.used)
		video_dev_set_format(&cam->vc2);

	if (cam->vc3.used)
		video_dev_set_format(&cam->vc3);

	return 0;
}

int camera_video_query_ctrls(struct camera_dev *cam)
{
	if (cam->raw.used)
		video_dev_query_ctrls(&cam->raw);

#ifdef CAPTURE_SUPPORT_VIEW
	if (cam->view.used)
		video_dev_query_ctrls(&cam->view);

	if (cam->cap.used)
		video_dev_query_ctrls(&cam->cap);

	if (cam->record.used)
		video_dev_query_ctrls(&cam->record);

	if (cam->disp.used)
		video_dev_query_ctrls(&cam->disp);
#endif

	return 0;
}

int camera_data_cfg_format(struct camera_dev *cam)
{
	struct video_dev *vdev = NULL;
	struct format_info *fmt = NULL;

	if (cam->stats.used) {
		vdev = &cam->stats;
		fmt = &vdev->format;

		fmt->width = 0;
		fmt->height = 0;
		fmt->fmt = V4L2_META_FMT_C4ISP_STATS;
		video_dev_set_format(vdev);
	}

	if (cam->param.used) {
		vdev = &cam->param;
		fmt = &vdev->format;

		fmt->width = 0;
		fmt->height = 0;
		fmt->fmt = V4L2_META_FMT_C4ISP_PARAMS;
		video_dev_set_format(vdev);
	}

	return 0;
}

int camera_video_alloc_buffs(struct camera_dev *cam)
{
	if (cam->stats.used) {
		video_dev_splane_alloc_buffs(&cam->stats, 4);
		video_dev_export_buffs(&cam->stats);
		video_dev_mmap_buffs(&cam->stats);
	}

	if (cam->imp.used) {
		video_dev_mplane_alloc_buffs(&cam->imp, 4);

		if (cam->imp.mem_type == V4L2_MEMORY_MMAP) {
			video_dev_export_buffs(&cam->imp);
			video_dev_mmap_buffs(&cam->imp);
		}
	}

	if (cam->param.used) {
		video_dev_splane_alloc_buffs(&cam->param, 4);
		video_dev_export_buffs(&cam->param);
		video_dev_mmap_buffs(&cam->param);
	}

	if (cam->raw.used) {
		video_dev_mplane_alloc_buffs(&cam->raw, 4);
		video_dev_export_buffs(&cam->raw);
		video_dev_mmap_buffs(&cam->raw);
	}

#ifdef CAPTURE_SUPPORT_VIEW
	if (cam->view.used) {
		video_dev_mplane_alloc_buffs(&cam->view, 4);
		video_dev_export_buffs(&cam->view);
		video_dev_mmap_buffs(&cam->view);
	}

	if (cam->cap.used) {
		video_dev_mplane_alloc_buffs(&cam->cap, 4);
		video_dev_export_buffs(&cam->cap);
		video_dev_mmap_buffs(&cam->cap);
	}

	if (cam->record.used) {
		video_dev_mplane_alloc_buffs(&cam->record, 4);
		video_dev_export_buffs(&cam->record);
		video_dev_mmap_buffs(&cam->record);
	}

	if (cam->disp.used) {
		video_dev_mplane_alloc_buffs(&cam->disp, 4);
		video_dev_export_buffs(&cam->disp);
		video_dev_mmap_buffs(&cam->disp);
	}
#endif

	if (cam->vc0.used) {
		video_dev_mplane_alloc_buffs(&cam->vc0, 4);
		video_dev_export_buffs(&cam->vc0);
		video_dev_mmap_buffs(&cam->vc0);
	}

	if (cam->vc1.used) {
		video_dev_mplane_alloc_buffs(&cam->vc1, 4);
		video_dev_export_buffs(&cam->vc1);
		video_dev_mmap_buffs(&cam->vc1);
	}

	if (cam->vc2.used) {
		video_dev_mplane_alloc_buffs(&cam->vc2, 4);
		video_dev_export_buffs(&cam->vc2);
		video_dev_mmap_buffs(&cam->vc2);
	}

	if (cam->vc3.used) {
		video_dev_mplane_alloc_buffs(&cam->vc3, 4);
		video_dev_export_buffs(&cam->vc3);
		video_dev_mmap_buffs(&cam->vc3);
	}

	return 0;
}

int camera_video_queue_buffs(struct camera_dev *cam)
{
	struct buffer_t *buff;

	if (cam->stats.used)
		camera_video_init_buff_queue(&cam->stats);

	if (cam->imp.used)
		camera_video_init_buff_queue(&cam->imp);

	if (cam->param.used) {
		camera_video_init_buff_queue(&cam->param);

		buff = video_get_list_buff(&cam->param);
		if (buff) {
			camera_alg_param_init(cam, buff);
			video_delete_list_buff(&cam->param, buff);
			video_dev_queue_buff(&cam->param, buff);
		}
	}

	if (cam->raw.used)
		camera_video_init_buff_queue(&cam->raw);

#ifdef CAPTURE_SUPPORT_VIEW
	if (cam->view.used)
		camera_video_init_buff_queue(&cam->view);

	if (cam->cap.used)
		camera_video_init_buff_queue(&cam->cap);

	if (cam->record.used)
		camera_video_init_buff_queue(&cam->record);

	if (cam->disp.used)
		camera_video_init_buff_queue(&cam->disp);
#endif

	if (cam->vc0.used)
		camera_video_init_buff_queue(&cam->vc0);

	if (cam->vc1.used)
		camera_video_init_buff_queue(&cam->vc1);

	if (cam->vc2.used)
		camera_video_init_buff_queue(&cam->vc2);

	if (cam->vc3.used)
		camera_video_init_buff_queue(&cam->vc3);

	return 0;
}

int camera_video_create_thread(struct camera_dev *cam)
{
	int ret = 0;
	ret = pthread_create(&cam->tid, NULL, camera_video_event_thread, (void *)cam);
	if (ret)
		printf("Error to create camera thread\n");

	return ret;
}

int camera_video_stream_on_vc(struct camera_dev *cam)
{
	if (cam->vc0.used)
		video_dev_stream_on(&cam->vc0);

	if (cam->vc1.used)
		video_dev_stream_on(&cam->vc1);

	if (cam->vc2.used)
		video_dev_stream_on(&cam->vc2);

	if (cam->vc3.used)
		video_dev_stream_on(&cam->vc3);

	return 0;
}


int camera_video_stream_on(struct camera_dev *cam)
{
	if (cam->stats.used)
		video_dev_stream_on(&cam->stats);

	if (cam->imp.used)
		video_dev_stream_on(&cam->imp);

	if (cam->param.used)
		video_dev_stream_on(&cam->param);

#ifdef CAPTURE_SUPPORT_VIEW
	if (cam->view.used)
		video_dev_stream_on(&cam->view);

	if (cam->cap.used)
		video_dev_stream_on(&cam->cap);

	if (cam->record.used)
		video_dev_stream_on(&cam->record);

	if (cam->disp.used)
		video_dev_stream_on(&cam->disp);
#endif

	/* RAW video must be streamed last */
	if (cam->raw.used)
		video_dev_stream_on(&cam->raw);

	return 0;
}

void camera_video_exit_thread(struct camera_dev *cam)
{
	pthread_join(cam->tid, NULL);
}

void camera_video_stream_off(struct camera_dev *cam)
{
	if (cam->stats.used)
		video_dev_stream_off(&cam->stats);

	if (cam->imp.used)
		video_dev_stream_off(&cam->imp);

	if (cam->param.used)
		video_dev_stream_off(&cam->param);

	if (cam->raw.used)
		video_dev_stream_off(&cam->raw);

#ifdef CAPTURE_SUPPORT_VIEW
	if (cam->view.used)
		video_dev_stream_off(&cam->view);

	if (cam->cap.used)
		video_dev_stream_off(&cam->cap);

	if (cam->record.used)
		video_dev_stream_off(&cam->record);

	if (cam->disp.used)
		video_dev_stream_off(&cam->disp);
#endif

	if (cam->vc0.used)
		video_dev_stream_off(&cam->vc0);

	if (cam->vc1.used)
		video_dev_stream_off(&cam->vc1);

	if (cam->vc2.used)
		video_dev_stream_off(&cam->vc2);

	if (cam->vc3.used)
		video_dev_stream_off(&cam->vc3);
}

void camera_video_release_buffs(struct camera_dev *cam)
{
	if (cam->raw.used) {
		video_dev_unmap_buffs(&cam->raw);
		video_dev_close_buffs(&cam->raw);
		video_dev_release_buffs(&cam->raw);
	}

#ifdef CAPTURE_SUPPORT_VIEW
	if (cam->view.used) {
		video_dev_unmap_buffs(&cam->view);
		video_dev_close_buffs(&cam->view);
		video_dev_release_buffs(&cam->view);
	}

	if (cam->cap.used) {
		video_dev_unmap_buffs(&cam->cap);
		video_dev_close_buffs(&cam->cap);
		video_dev_release_buffs(&cam->cap);
	}

	if (cam->record.used) {
		video_dev_unmap_buffs(&cam->record);
		video_dev_close_buffs(&cam->record);
		video_dev_release_buffs(&cam->record);
	}

	if (cam->disp.used) {
		video_dev_unmap_buffs(&cam->disp);
		video_dev_close_buffs(&cam->disp);
		video_dev_release_buffs(&cam->disp);
	}
#endif

	if (cam->stats.used) {
		video_dev_unmap_buffs(&cam->stats);
		video_dev_close_buffs(&cam->stats);
		video_dev_release_buffs(&cam->stats);
	}

	if (cam->imp.used) {
		if (cam->imp.mem_type == V4L2_MEMORY_MMAP) {
			video_dev_unmap_buffs(&cam->imp);
			video_dev_close_buffs(&cam->imp);
		}

		video_dev_release_buffs(&cam->imp);
	}

	if (cam->param.used) {
		video_dev_unmap_buffs(&cam->param);
		video_dev_close_buffs(&cam->param);
		video_dev_release_buffs(&cam->param);
	}

	if (cam->vc0.used) {
		video_dev_unmap_buffs(&cam->vc0);
		video_dev_close_buffs(&cam->vc0);
		video_dev_release_buffs(&cam->vc0);
	}

	if (cam->vc1.used) {
		video_dev_unmap_buffs(&cam->vc1);
		video_dev_close_buffs(&cam->vc1);
		video_dev_release_buffs(&cam->vc1);
	}

	if (cam->vc2.used) {
		video_dev_unmap_buffs(&cam->vc2);
		video_dev_close_buffs(&cam->vc2);
		video_dev_release_buffs(&cam->vc2);
	}

	if (cam->vc3.used) {
		video_dev_unmap_buffs(&cam->vc3);
		video_dev_close_buffs(&cam->vc3);
		video_dev_release_buffs(&cam->vc3);
	}
}

void camera_dev_disable_links(struct camera_dev *cam)
{
	media_disable_entity_links(cam, &cam->csiphy.entity);
	media_disable_entity_links(cam, &cam->adap.entity);
	media_disable_entity_links(cam, &cam->ipp.entity);
	media_disable_entity_links(cam, &cam->isp.entity);
	media_disable_entity_links(cam, &cam->view_rsz.entity);
	media_disable_entity_links(cam, &cam->cap_rsz.entity);
	media_disable_entity_links(cam, &cam->record_rsz.entity);
	media_disable_entity_links(cam, &cam->disp_rsz.entity);
	media_disable_entity_links(cam, &cam->stats.entity);
	media_disable_entity_links(cam, &cam->imp.entity);
	media_disable_entity_links(cam, &cam->raw.entity);
	media_disable_entity_links(cam, &cam->view.entity);
	media_disable_entity_links(cam, &cam->cap.entity);
	media_disable_entity_links(cam, &cam->record.entity);
	media_disable_entity_links(cam, &cam->disp.entity);
	media_disable_entity_links(cam, &cam->vc0.entity);
	media_disable_entity_links(cam, &cam->vc1.entity);
	media_disable_entity_links(cam, &cam->vc2.entity);
	media_disable_entity_links(cam, &cam->vc3.entity);
}

void camera_dev_deinit(struct camera_dev *cam)
{
	subdev_close(&cam->sensor);
	subdev_close(&cam->csiphy);
	subdev_close(&cam->adap);
	subdev_close(&cam->ipp);
	subdev_close(&cam->isp);
	subdev_close(&cam->view_rsz);
	subdev_close(&cam->cap_rsz);
	subdev_close(&cam->record_rsz);
	subdev_close(&cam->disp_rsz);

	if (cam->stats.used)
		video_dev_close(&cam->stats);

	if (cam->imp.used)
		video_dev_close(&cam->imp);

	if (cam->param.used)
		video_dev_close(&cam->param);

	if (cam->raw.used)
		video_dev_close(&cam->raw);

#ifdef CAPTURE_SUPPORT_VIEW
	if (cam->view.used)
		video_dev_close(&cam->view);

	if (cam->cap.used)
		video_dev_close(&cam->cap);

	if (cam->record.used)
		video_dev_close(&cam->record);

	if (cam->disp.used)
		video_dev_close(&cam->disp);
#endif

	if (cam->vc0.used)
		video_dev_close(&cam->vc0);

	if (cam->vc1.used)
		video_dev_close(&cam->vc1);

	if (cam->vc2.used)
		video_dev_close(&cam->vc2);

	if (cam->vc3.used)
		video_dev_close(&cam->vc3);
}