/*
 * Xilinx Video Library - Generic V4L2 support
 *
 * Copyright (C) 2014-2016 Ideas on board Oy
 *
 * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
 *
 * This library is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or (at
 * your option) any later version.
 *
 * This library 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.
 */

#include <stdio.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdint.h>
#include <stdlib.h>
#include <errno.h>
#include <getopt.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/select.h>
#include <sys/time.h>

#include <linux/videodev2.h>

#include <xlnx-buffers.h>
#include <xlnx-list.h>
#include <xlnx-tools.h>

#include "v4l2.h"

#ifndef V4L2_BUF_FLAG_ERROR
#define V4L2_BUF_FLAG_ERROR	0x0040
#endif

struct v4l2_ival_desc {
	struct v4l2_fract min;
	struct v4l2_fract max;
	struct v4l2_fract step;

	struct list_entry list;
};

struct v4l2_frame_desc {
	unsigned int min_width;
	unsigned int min_height;
	unsigned int max_width;
	unsigned int max_height;
	unsigned int step_width;
	unsigned int step_height;

	struct list_entry list;
	struct list_entry ivals;
};

struct v4l2_format_desc {
	unsigned int pixelformat;

	struct list_entry list;
	struct list_entry frames;
};

/* -----------------------------------------------------------------------------
 * Formats enumeration
 */

static int
v4l2_enum_frame_intervals(struct v4l2_device *dev, struct v4l2_format_desc *format,
	struct v4l2_frame_desc *frame)
{
	struct v4l2_ival_desc *ival;
	unsigned int i;
	int ret;

	for (i = 0; ; ++i) {
		struct v4l2_frmivalenum ivalenum;

		memset(&ivalenum, 0, sizeof ivalenum);
		ivalenum.index = i;
		ivalenum.pixel_format = format->pixelformat;
		ivalenum.width = frame->min_width;
		ivalenum.height = frame->min_height;
		ret = ioctl(dev->fd, VIDIOC_ENUM_FRAMEINTERVALS, &ivalenum);
		if (ret < 0)
			break;

		if (i != ivalenum.index)
			printf("Warning: driver returned wrong ival index "
				"%u.\n", ivalenum.index);
		if (format->pixelformat != ivalenum.pixel_format)
			printf("Warning: driver returned wrong ival pixel "
				"format %08x.\n", ivalenum.pixel_format);
		if (frame->min_width != ivalenum.width)
			printf("Warning: driver returned wrong ival width "
				"%u.\n", ivalenum.width);
		if (frame->min_height != ivalenum.height)
			printf("Warning: driver returned wrong ival height "
				"%u.\n", ivalenum.height);

		ival = malloc(sizeof *ival);
		if (ival == NULL)
			return -ENOMEM;

		memset(ival, 0, sizeof *ival);

		switch (ivalenum.type) {
		case V4L2_FRMIVAL_TYPE_DISCRETE:
			ival->min = ivalenum.discrete;
			ival->max = ivalenum.discrete;
			ival->step.numerator = 1;
			ival->step.denominator = 1;
			break;

		case V4L2_FRMIVAL_TYPE_STEPWISE:
			ival->min = ivalenum.stepwise.min;
			ival->max = ivalenum.stepwise.max;
			ival->step = ivalenum.stepwise.step;
			break;

		default:
			printf("Error: driver returned invalid frame ival "
				"type %u\n", ivalenum.type);
			return -EINVAL;
		}

		list_append(&ival->list, &frame->ivals);
	}

	return 0;
}

static int
v4l2_enum_frame_sizes(struct v4l2_device *dev, struct v4l2_format_desc *format)
{
	struct v4l2_frame_desc *frame;
	unsigned int i;
	int ret;

	for (i = 0; ; ++i) {
		struct v4l2_frmsizeenum frmenum;

		memset(&frmenum, 0, sizeof frmenum);
		frmenum.index = i;
		frmenum.pixel_format = format->pixelformat;

		ret = ioctl(dev->fd, VIDIOC_ENUM_FRAMESIZES, &frmenum);
		if (ret < 0)
			break;

		if (i != frmenum.index)
			printf("Warning: driver returned wrong frame index "
				"%u.\n", frmenum.index);
		if (format->pixelformat != frmenum.pixel_format)
			printf("Warning: driver returned wrong frame pixel "
				"format %08x.\n", frmenum.pixel_format);

		frame = malloc(sizeof *frame);
		if (frame == NULL)
			return -ENOMEM;

		memset(frame, 0, sizeof *frame);

		list_init(&frame->ivals);
		frame->step_width = 1;
		frame->step_height = 1;

		switch (frmenum.type) {
		case V4L2_FRMSIZE_TYPE_DISCRETE:
			frame->min_width = frmenum.discrete.width;
			frame->min_height = frmenum.discrete.height;
			frame->max_width = frmenum.discrete.width;
			frame->max_height = frmenum.discrete.height;
			break;

		case V4L2_FRMSIZE_TYPE_STEPWISE:
			frame->step_width = frmenum.stepwise.step_width;
			frame->step_height = frmenum.stepwise.step_height;
		case V4L2_FRMSIZE_TYPE_CONTINUOUS:
			frame->min_width = frmenum.stepwise.min_width;
			frame->min_height = frmenum.stepwise.min_height;
			frame->max_width = frmenum.stepwise.max_width;
			frame->max_height = frmenum.stepwise.max_height;
			break;

		default:
			printf("Error: driver returned invalid frame size "
				"type %u\n", frmenum.type);
			return -EINVAL;
		}

		list_append(&frame->list, &format->frames);

		ret = v4l2_enum_frame_intervals(dev, format, frame);
		if (ret < 0)
			return ret;
	}

	return 0;
}
static int v4l2_enum_formats(struct v4l2_device *dev)
{
	struct v4l2_format_desc *format;
	unsigned int i;
	int ret;

	for (i = 0; ; ++i) {
		struct v4l2_fmtdesc fmtenum;

		memset(&fmtenum, 0, sizeof fmtenum);
		fmtenum.index = i;
		fmtenum.type = dev->type;

		ret = ioctl(dev->fd, VIDIOC_ENUM_FMT, &fmtenum);
		if (ret < 0)
			break;

		if (i != fmtenum.index)
			printf("Warning: driver returned wrong format index "
				"%u.\n", fmtenum.index);
		if (dev->type != fmtenum.type)
			printf("Warning: driver returned wrong format type "
				"%u.\n", fmtenum.type);

		format = malloc(sizeof *format);
		if (format == NULL)
			return -ENOMEM;

		memset(format, 0, sizeof *format);

		list_init(&format->frames);
		format->pixelformat = fmtenum.pixelformat;

		list_append(&format->list, &dev->formats);

		ret = v4l2_enum_frame_sizes(dev, format);
		if (ret < 0)
			return ret;
	}

	return 0;
}

/* -----------------------------------------------------------------------------
 * Open/close
 */

struct v4l2_device *v4l2_open(const char *devname)
{
	struct v4l2_device *dev;
	struct v4l2_capability cap;
	int ret;

	dev = malloc(sizeof *dev);
	if (dev == NULL)
		return NULL;

	memset(dev, 0, sizeof *dev);
	dev->fd = -1;
	dev->name = strdup(devname);
	list_init(&dev->formats);

	dev->fd = open(devname, O_RDWR | O_NONBLOCK);
	if (dev->fd < 0) {
		printf("Error opening device %s: %d.\n", devname, errno);
		v4l2_close(dev);
		return NULL;
	}

	memset(&cap, 0, sizeof cap);
	ret = ioctl(dev->fd, VIDIOC_QUERYCAP, &cap);
	if (ret < 0) {
		printf("Error opening device %s: unable to query "
			"device.\n", devname);
		v4l2_close(dev);
		return NULL;
	}

	if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
		dev->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	else if (cap.capabilities & V4L2_CAP_VIDEO_OUTPUT)
		dev->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
	else {
		printf("Error opening device %s: neither video capture "
			"nor video output supported.\n", devname);
		v4l2_close(dev);
		return NULL;
	}

	ret = v4l2_enum_formats(dev);
	if (ret < 0) {
		printf("Error opening device %s: unable to enumerate "
			"formats.\n", devname);
		v4l2_close(dev);
		return NULL;
	}

	printf("Device %s opened: %s (%s).\n", devname, cap.card, cap.bus_info);

	return dev;
}

void v4l2_close(struct v4l2_device *dev)
{
	struct v4l2_format_desc *format, *next_fmt;
	struct v4l2_frame_desc *frame, *next_frm;
	struct v4l2_ival_desc *ival, *next_ival;

	if (dev == NULL)
		return;

	list_for_each_entry_safe(format, next_fmt, &dev->formats, list) {
		list_for_each_entry_safe(frame, next_frm, &format->frames, list) {
			list_for_each_entry_safe(ival, next_ival, &frame->ivals, list) {
				free(ival);
			}
			free(frame);
		}
		free(format);
	}

	free(dev->name);
	close(dev->fd);
}

/* -----------------------------------------------------------------------------
 * Controls
 */

int v4l2_get_control(struct v4l2_device *dev, unsigned int id, int32_t *value)
{
	struct v4l2_control ctrl;
	int ret;

	ctrl.id = id;

	ret = ioctl(dev->fd, VIDIOC_G_CTRL, &ctrl);
	if (ret < 0) {
		printf("%s: unable to get control (%d).\n", dev->name, errno);
		return -errno;
	}

	*value = ctrl.value;
	return 0;
}

int v4l2_set_control(struct v4l2_device *dev, unsigned int id, int32_t *value)
{
	struct v4l2_control ctrl;
	int ret;

	ctrl.id = id;
	ctrl.value = *value;

	ret = ioctl(dev->fd, VIDIOC_S_CTRL, &ctrl);
	if (ret < 0) {
		printf("%s: unable to set control (%d).\n", dev->name, errno);
		return -errno;
	}

	*value = ctrl.value;
	return 0;
}

int v4l2_get_controls(struct v4l2_device *dev, unsigned int count,
		      struct v4l2_ext_control *ctrls)
{
	struct v4l2_ext_controls controls;
	int ret;

	memset(&controls, 0, sizeof controls);
	controls.count = count;
	controls.controls = ctrls;

	ret = ioctl(dev->fd, VIDIOC_G_EXT_CTRLS, &controls);
	if (ret < 0)
		printf("%s: unable to get multiple controls (%d).\n", dev->name,
		       errno);

	return ret;
}

int v4l2_set_controls(struct v4l2_device *dev, unsigned int count,
		      struct v4l2_ext_control *ctrls)
{
	struct v4l2_ext_controls controls;
	int ret;

	memset(&controls, 0, sizeof controls);
	controls.count = count;
	controls.controls = ctrls;

	ret = ioctl(dev->fd, VIDIOC_S_EXT_CTRLS, &controls);
	if (ret < 0)
		printf("%s: unable to set multiple controls (%d).\n", dev->name,
		       errno);

	return ret;
}

/* -----------------------------------------------------------------------------
 * Formats and frame rates
 */

int v4l2_get_crop(struct v4l2_device *dev, struct v4l2_rect *rect)
{
	struct v4l2_crop crop;
	int ret;

	memset(&crop, 0, sizeof crop);
	crop.type = dev->type;

	ret = ioctl(dev->fd, VIDIOC_G_CROP, &crop);
	if (ret < 0) {
		printf("%s: unable to get crop rectangle (%d).\n", dev->name,
		       errno);
		return -errno;
	}

	dev->crop = crop.c;
	*rect = crop.c;

	return 0;
}

int v4l2_set_crop(struct v4l2_device *dev, struct v4l2_rect *rect)
{
	struct v4l2_crop crop;
	int ret;

	memset(&crop, 0, sizeof crop);
	crop.type = dev->type;
	crop.c = *rect;

	ret = ioctl(dev->fd, VIDIOC_S_CROP, &crop);
	if (ret < 0) {
		printf("%s: unable to set crop rectangle (%d).\n", dev->name,
		       errno);
		return -errno;
	}

	dev->crop = crop.c;
	*rect = crop.c;

	return 0;
}

int v4l2_get_format(struct v4l2_device *dev, struct v4l2_pix_format *format)
{
	struct v4l2_format fmt;
	int ret;

	memset(&fmt, 0, sizeof fmt);
	fmt.type = dev->type;

	ret = ioctl(dev->fd, VIDIOC_G_FMT, &fmt);
	if (ret < 0) {
		printf("%s: unable to get format (%d).\n", dev->name, errno);
		return -errno;
	}

	dev->format = fmt.fmt.pix;
	*format = fmt.fmt.pix;

	return 0;
}

int v4l2_set_format(struct v4l2_device *dev, struct v4l2_pix_format *format)
{
	struct v4l2_format fmt;
	int ret;

	memset(&fmt, 0, sizeof fmt);
	fmt.type = dev->type;
	fmt.fmt.pix.width = format->width;
	fmt.fmt.pix.height = format->height;
	fmt.fmt.pix.pixelformat = format->pixelformat;
	fmt.fmt.pix.field = V4L2_FIELD_ANY;

	ret = ioctl(dev->fd, VIDIOC_S_FMT, &fmt);
	if (ret < 0) {
		printf("%s: unable to set format (%d).\n", dev->name, errno);
		return -errno;
	}

	dev->format = fmt.fmt.pix;
	*format = fmt.fmt.pix;

	return 0;
}

/* -----------------------------------------------------------------------------
 * Buffers management
 */

int v4l2_alloc_buffers(struct v4l2_device *dev,
		       struct xlnx_video_buffers_pool *pool,
		       enum v4l2_memory memtype)
{
	struct v4l2_requestbuffers rb;
	struct v4l2_buffer buf;
	unsigned int i;
	int ret;

	dev->memtype = memtype;
	dev->pool = pool;

	memset(&rb, 0, sizeof rb);
	rb.count = pool->nbufs;
	rb.type = dev->type;
	rb.memory = memtype;

	ret = ioctl(dev->fd, VIDIOC_REQBUFS, &rb);
	if (ret < 0) {
		printf("%s: unable to request buffers (%d).\n", dev->name,
		       errno);
		ret = -errno;
		goto done;
	}

	if (rb.count > pool->nbufs) {
		printf("%s: driver needs more buffers (%u) than available (%u).\n",
		       dev->name, rb.count, pool->nbufs);
		ret = -E2BIG;
		goto done;
	}

	dev->nbufs = rb.count;

	printf("%s: %u buffers requested.\n", dev->name, rb.count);

	/* Map the buffers. */
	for (i = 0; i < rb.count; ++i) {
		memset(&buf, 0, sizeof buf);
		buf.index = i;
		buf.type = dev->type;
		buf.memory = memtype;
		ret = ioctl(dev->fd, VIDIOC_QUERYBUF, &buf);
		if (ret < 0) {
			printf("%s: unable to query buffer %u (%d).\n",
			       dev->name, i, errno);
			ret = -errno;
			goto done;
		}

		switch (memtype) {
		case V4L2_MEMORY_MMAP:
			pool->buffers[i].mem = mmap(0, buf.length, PROT_READ | PROT_WRITE,
					      MAP_SHARED, dev->fd, buf.m.offset);
			if (pool->buffers[i].mem == MAP_FAILED) {
				printf("%s: unable to map buffer %u (%d)\n",
				       dev->name, i, errno);
				ret = -errno;
				goto done;
			}
			pool->buffers[i].size = buf.length;
			printf("%s: buffer %u mapped at address %p.\n",
			       dev->name, i, pool->buffers[i].mem);
			break;

		case V4L2_MEMORY_USERPTR:
			if (pool->buffers[i].size < buf.length) {
				printf("%s: buffer %u too small (%u bytes required, "
				       "%u bytes available.\n", dev->name, i,
				       buf.length, pool->buffers[i].size);
				ret = -EINVAL;
				goto done;
			}

			printf("%s: buffer %u valid.\n", dev->name, i);
			break;

		default:
			break;
		}
	}

	ret = 0;

done:
	if (ret < 0)
		v4l2_free_buffers(dev);

	return ret;
}

int v4l2_free_buffers(struct v4l2_device *dev)
{
	struct v4l2_requestbuffers rb;
	unsigned int i;
	int ret;

	if (dev->pool == NULL)
		return 0;

	if (dev->memtype == V4L2_MEMORY_MMAP) {
		for (i = 0; i < dev->nbufs; ++i) {
			struct xlnx_video_buffer *buffer = &dev->pool->buffers[i];

			if (buffer->mem == NULL)
				continue;

			ret = munmap(buffer->mem, buffer->size);
			if (ret < 0) {
				printf("%s: unable to unmap buffer %u (%d)\n",
				       dev->name, i, errno);
				return -errno;
			}

			buffer->mem = NULL;
			buffer->size = 0;
		}
	}

	memset(&rb, 0, sizeof rb);
	rb.count = 0;
	rb.type = dev->type;
	rb.memory = dev->memtype;

	ret = ioctl(dev->fd, VIDIOC_REQBUFS, &rb);
	if (ret < 0) {
		printf("%s: unable to release buffers (%d)\n", dev->name,
		       errno);
		return -errno;
	}

	dev->nbufs = 0;
	dev->pool = NULL;

	return 0;
}

int v4l2_dequeue_buffer(struct v4l2_device *dev, struct xlnx_video_buffer *buffer)
{
	struct v4l2_buffer buf;
	int ret;

	memset(&buf, 0, sizeof buf);
	buf.type = dev->type;
	buf.memory = dev->memtype;

	ret = ioctl(dev->fd, VIDIOC_DQBUF, &buf);
	if (ret < 0) {
		printf("%s: unable to dequeue buffer index %u/%u (%d)\n",
		       dev->name, buf.index, dev->nbufs, errno);
		return -errno;
	}

	*buffer = dev->pool->buffers[buf.index];
	buffer->bytesused = buf.bytesused;
	buffer->timestamp = buf.timestamp;
	buffer->error = !!(buf.flags & V4L2_BUF_FLAG_ERROR);

	return 0;
}

int v4l2_queue_buffer(struct v4l2_device *dev, struct xlnx_video_buffer *buffer)
{
	struct v4l2_buffer buf;
	int ret;

	if (buffer->index >= dev->nbufs)
		return -EINVAL;

	memset(&buf, 0, sizeof buf);
	buf.index = buffer->index;
	buf.type = dev->type;
	buf.memory = dev->memtype;

	if (dev->memtype == V4L2_MEMORY_USERPTR)
		buf.m.userptr = (unsigned long)dev->pool->buffers[buffer->index].mem;

	buf.length = dev->pool->buffers[buffer->index].size;

	if (dev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
		buf.bytesused = buffer->bytesused;

	ret = ioctl(dev->fd, VIDIOC_QBUF, &buf);
	if (ret < 0) {
		printf("%s: unable to queue buffer index %u/%u (%d)\n",
		       dev->name, buf.index, dev->nbufs, errno);
		return -errno;
	}

	return 0;
}

/* -----------------------------------------------------------------------------
 * Stream management
 */

int v4l2_stream_on(struct v4l2_device *dev)
{
	int type = dev->type;
	int ret;

	ret = ioctl(dev->fd, VIDIOC_STREAMON, &type);
	if (ret < 0)
		return -errno;

	return 0;
}

int v4l2_stream_off(struct v4l2_device *dev)
{
	int type = dev->type;
	int ret;

	ret = ioctl(dev->fd, VIDIOC_STREAMOFF, &type);
	if (ret < 0)
		return -errno;

	return 0;
}