AM5748: Need help to stream video from vin1a (VIP 1 port A) to HDMI display port (DSS)

/*
 * Demo application for DMA buffer sharing between V4L2 and DRM
 * Tomasz Stanislawski <t.stanisl...@samsung.com>
 *
 * Copyright 2012 Samsung Electronics Co., Ltd.
 *
 * Licensed 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.
 *
 */

#include <errno.h>
#include <fcntl.h>
#include <math.h>
#include <poll.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>

#include <drm.h>
#include <drm_mode.h>

#include <linux/videodev2.h>

#include <xf86drm.h>
#include <xf86drmMode.h>

#define ERRSTR strerror(errno)

#define BYE_ON(cond, ...) \
do { \
	if (cond) { \
		int errsv = errno; \
		fprintf(stderr, "ERROR(%s:%d) : ", \
			__FILE__, __LINE__); \
		errno = errsv; \
		fprintf(stderr,  __VA_ARGS__); \
		exit(0); \
	} \
} while(0)

static inline int warn(const char *file, int line, const char *fmt, ...)
{
	int errsv = errno;
	va_list va;
	va_start(va, fmt);
	fprintf(stderr, "WARN(%s:%d): ", file, line);
	vfprintf(stderr, fmt, va);
	va_end(va);
	errno = errsv;
	return 1;
}

#define WARN_ON(cond, ...) \
	((cond) ? warn(__FILE__, __LINE__, __VA_ARGS__) : 0)

struct setup {
	char module[32];
	int conId;
	uint32_t crtcId;
	int crtcIdx;
	uint32_t planeId;
	char video[32];
	unsigned int w, h;
	unsigned int use_wh : 1;
	unsigned int in_fourcc;
	unsigned int out_fourcc;
	unsigned int buffer_count;
	unsigned int use_crop : 1;
	unsigned int use_compose : 1;
	struct v4l2_rect crop;
	struct v4l2_rect compose;
};

struct buffer {
	unsigned int bo_handle;
	unsigned int fb_handle;
	int dbuf_fd;
};

struct stream {
	int v4lfd;
	int current_buffer;
	int buffer_count;
	struct buffer *buffer;
} stream;

static char *c_fmt(unsigned int fmt)
{
    static char fmt_buf[5];
    char *p_fmt = (char *)(&fmt);
    fmt_buf[4] = 0;
    for (int i=0; i<4; i++) {
        fmt_buf[i] = p_fmt[i];
    }
    return &fmt_buf[0];
}

static void usage(char *name)
{
	fprintf(stderr, "usage: %s [-Moisth]\n", name);
	fprintf(stderr, "\t-M <drm-module>\tset DRM module\n");
	fprintf(stderr, "\t-o <connector_id>:<crtc_id>\tchoose a connector/crtc\n");
	fprintf(stderr, "\t-i <video-node>\tset video node like /dev/video*\n");
	fprintf(stderr, "\t-S <width,height>\tset input resolution\n");
	fprintf(stderr, "\t-f <fourcc>\tset input format using 4cc\n");
	fprintf(stderr, "\t-F <fourcc>\tset output format using 4cc\n");
	fprintf(stderr, "\t-s <width,height>@<left,top>\tset crop area\n");
	fprintf(stderr, "\t-t <width,height>@<left,top>\tset compose area\n");
	fprintf(stderr, "\t-b buffer_count\tset number of buffers\n");
	fprintf(stderr, "\t-h\tshow this help\n");
	fprintf(stderr, "\n\tDefault is to dump all info.\n");
}

static inline int parse_rect(char *s, struct v4l2_rect *r)
{
	return sscanf(s, "%d,%d@%d,%d", &r->width, &r->height,
		&r->top, &r->left) != 4;
}

static int parse_args(int argc, char *argv[], struct setup *s)
{
	if (argc <= 1)
		usage(argv[0]);

	int c, ret;
	memset(s, 0, sizeof(*s));

	while ((c = getopt(argc, argv, "M:o:i:S:f:F:s:t:b:h")) != -1) {
		switch (c) {
		case 'M':
			strncpy(s->module, optarg, 31);
			break;
		case 'o':
			ret = sscanf(optarg, "%u:%u", &s->conId, &s->crtcId);
			if (WARN_ON(ret != 2, "incorrect con/ctrc description\n"))
				return -1;
			break;
		case 'i':
			strncpy(s->video, optarg, 31);
			break;
		case 'S':
			ret = sscanf(optarg, "%u,%u", &s->w, &s->h);
			if (WARN_ON(ret != 2, "incorrect input size\n"))
				return -1;
			s->use_wh = 1;
			break;
		case 'f':
			if (WARN_ON(strlen(optarg) != 4, "invalid fourcc\n"))
				return -1;
			s->in_fourcc = ((unsigned)optarg[0] << 0) |
				((unsigned)optarg[1] << 8) |
				((unsigned)optarg[2] << 16) |
				((unsigned)optarg[3] << 24);
			break;
		case 'F':
			if (WARN_ON(strlen(optarg) != 4, "invalid fourcc\n"))
				return -1;
			s->out_fourcc = ((unsigned)optarg[0] << 0) |
				((unsigned)optarg[1] << 8) |
				((unsigned)optarg[2] << 16) |
				((unsigned)optarg[3] << 24);
			break;
		case 's':
			ret = parse_rect(optarg, &s->crop);
			if (WARN_ON(ret, "incorrect crop area\n"))
				return -1;
			s->use_crop = 1;
			break;
		case 't':
			ret = parse_rect(optarg, &s->compose);
			if (WARN_ON(ret, "incorrect compose area\n"))
				return -1;
			s->use_compose = 1;
			break;
		case 'b':
			ret = sscanf(optarg, "%u", &s->buffer_count);
			if (WARN_ON(ret != 1, "incorrect buffer count\n"))
				return -1;
			break;
		case '?':
		case 'h':
			usage(argv[0]);
			return -1;
		}
	}

	return 0;
}

static int buffer_create(struct buffer *b, int drmfd, struct setup *s,
	uint64_t size, uint32_t pitch)
{
	struct drm_mode_create_dumb gem;
	struct drm_mode_destroy_dumb gem_destroy;
	int ret;

	memset(&gem, 0, sizeof gem);
	gem.width = s->w;
	gem.height = s->h;
	gem.bpp = 32;
	gem.size = size;
	ret = ioctl(drmfd, DRM_IOCTL_MODE_CREATE_DUMB, &gem);
	if (WARN_ON(ret, "CREATE_DUMB failed: %s\n", ERRSTR))
		return -1;
	printf("bo %u %ux%u bpp %u size %lu (%lu)\n", gem.handle, gem.width, gem.height, gem.bpp, (long)gem.size, (long)size);
	b->bo_handle = gem.handle;

	struct drm_prime_handle prime;
	memset(&prime, 0, sizeof prime);
	prime.handle = b->bo_handle;

	ret = ioctl(drmfd, DRM_IOCTL_PRIME_HANDLE_TO_FD, &prime);
	if (WARN_ON(ret, "PRIME_HANDLE_TO_FD failed: %s\n", ERRSTR))
		goto fail_gem;
	printf("dbuf_fd = %d\n", prime.fd);
	b->dbuf_fd = prime.fd;

	uint32_t offsets[4] = { 0 };
	uint32_t pitches[4] = { pitch };
	uint32_t bo_handles[4] = { b->bo_handle };
	unsigned int fourcc = s->out_fourcc;
	if (!fourcc)
		fourcc = s->in_fourcc;

	fprintf(stderr, "FB fourcc %c%c%c%c\n",
		fourcc,
		fourcc >> 8,
		fourcc >> 16,
		fourcc >> 24);

	ret = drmModeAddFB2(drmfd, s->w, s->h, fourcc, bo_handles,
		pitches, offsets, &b->fb_handle, 0);
	if (WARN_ON(ret, "drmModeAddFB2 failed: %s\n", ERRSTR))
		goto fail_prime;

	return 0;

fail_prime:
	close(b->dbuf_fd);

fail_gem:
	memset(&gem_destroy, 0, sizeof gem_destroy);
	gem_destroy.handle = b->bo_handle,
	ret = ioctl(drmfd, DRM_IOCTL_MODE_DESTROY_DUMB, &gem_destroy);
	WARN_ON(ret, "DESTROY_DUMB failed: %s\n", ERRSTR);

	return -1;
}

static int find_crtc(int drmfd, struct setup *s, uint32_t *con)
{
	int ret = -1;
	int i;
	drmModeRes *res = drmModeGetResources(drmfd);
	if (WARN_ON(!res, "drmModeGetResources failed: %s\n", ERRSTR))
		return -1;

	if (WARN_ON(res->count_crtcs <= 0, "drm: no crts\n"))
		goto fail_res;

	if (!s->conId) {
		fprintf(stderr,
			"No connector ID specified.  Choosing default from list:\n");

		for (i = 0; i < res->count_connectors; i++) {
			drmModeConnector *con =
				drmModeGetConnector(drmfd, res->connectors[i]);
			drmModeEncoder *enc = NULL;
			drmModeCrtc *crtc = NULL;

			if (con->encoder_id) {
				enc = drmModeGetEncoder(drmfd, con->encoder_id);
				if (enc->crtc_id) {
					crtc = drmModeGetCrtc(drmfd, enc->crtc_id);
				}
			}

			if (!s->conId && crtc) {
				s->conId = con->connector_id;
				s->crtcId = crtc->crtc_id;
			}

			printf("Connector %d (crtc %d): type %d, %dx%d%s\n",
			       con->connector_id,
			       crtc ? crtc->crtc_id : 0,
			       con->connector_type,
			       crtc ? crtc->width : 0,
			       crtc ? crtc->height : 0,
			       (s->conId == (int)con->connector_id ?
				" (chosen)" : ""));
		}

		if (!s->conId) {
			fprintf(stderr,
				"No suitable enabled connector found.\n");
			exit(1);
		}
	}

	s->crtcIdx = -1;

	for (i = 0; i < res->count_crtcs; ++i) {
		if (s->crtcId == res->crtcs[i]) {
			s->crtcIdx = i;
			break;
		}
	}

	if (WARN_ON(s->crtcIdx == -1, "drm: CRTC %u not found\n", s->crtcId))
		goto fail_res;

	if (WARN_ON(res->count_connectors <= 0, "drm: no connectors\n"))
		goto fail_res;

	drmModeConnector *c;
	c = drmModeGetConnector(drmfd, s->conId);
	if (WARN_ON(!c, "drmModeGetConnector failed: %s\n", ERRSTR))
		goto fail_res;

	if (WARN_ON(!c->count_modes, "connector supports no mode\n"))
		goto fail_conn;

	if (!s->use_compose) {
		drmModeCrtc *crtc = drmModeGetCrtc(drmfd, s->crtcId);
		s->compose.left = crtc->x;
		s->compose.top = crtc->y;
		s->compose.width = crtc->width;
		s->compose.height = crtc->height;
		drmModeFreeCrtc(crtc);
	}

	if (con)
		*con = c->connector_id;
	ret = 0;

fail_conn:
	drmModeFreeConnector(c);

fail_res:
	drmModeFreeResources(res);

	return ret;
}

static int find_plane(int drmfd, struct setup *s)
{
	drmModePlaneResPtr planes;
	drmModePlanePtr plane;
	unsigned int i;
	unsigned int j;
	int ret = 0;

	planes = drmModeGetPlaneResources(drmfd);
	if (WARN_ON(!planes, "drmModeGetPlaneResources failed: %s\n", ERRSTR))
		return -1;

        fprintf(stderr, "plane count [%d]\n", planes->count_planes);

	for (i = 0; i < planes->count_planes; ++i) {
		plane = drmModeGetPlane(drmfd, planes->planes[i]);
		if (WARN_ON(!plane, "drmModeGetPlane failed: %s\n", ERRSTR))
			break;

		if (!(plane->possible_crtcs & (1 << s->crtcIdx))) {
			drmModeFreePlane(plane);
		        fprintf(stderr,
			        "possible_crtcs not match\n");
			continue;
		}

		for (j = 0; j < plane->count_formats; ++j) {
			if (plane->formats[j] == s->out_fourcc)
				break;
                        fprintf(stderr, "ignoring plan format [%.4s]\n", c_fmt(plane->formats[j])); 
		}

		if (j == plane->count_formats) {
			drmModeFreePlane(plane);
		        fprintf(stderr,
			        "plane formats not contains s->out_fourcc[%.4s]\n", c_fmt(s->out_fourcc));
			continue;
		}

		s->planeId = plane->plane_id;
		drmModeFreePlane(plane);
		break;
	}

	if (i == planes->count_planes) {
                fprintf(stderr,
                        "cannot find one plane in %d planes\n", planes->count_planes);
		ret = -1;
        }

	drmModeFreePlaneResources(planes);
	return ret;
}

int main(int argc, char **argv)
{
	int ret;
	struct setup s;

	ret = parse_args(argc, argv, &s);
	BYE_ON(ret, "failed to parse arguments\n");
	BYE_ON(s.module[0] == 0, "DRM module is missing\n");
	BYE_ON(s.video[0] == 0, "video node is missing\n");

	int drmfd = drmOpen(s.module, NULL);
	BYE_ON(drmfd < 0, "drmOpen(%s) failed: %s\n", s.module, ERRSTR);

	int v4lfd = open(s.video, O_RDWR);
	BYE_ON(v4lfd < 0, "failed to open %s: %s\n", s.video, ERRSTR);

	struct v4l2_capability caps;
	memset(&caps, 0, sizeof caps);

	ret = ioctl(v4lfd, VIDIOC_QUERYCAP, &caps);
	BYE_ON(ret, "VIDIOC_QUERYCAP failed: %s\n", ERRSTR);

	/* TODO: add single plane support */
        printf("caps: info[%s] capabilities[0x%x] device_caps[0x%x]\n", caps.bus_info, caps.capabilities, caps.device_caps);
	BYE_ON(~caps.capabilities & V4L2_CAP_VIDEO_CAPTURE,
		"video: singleplanar capture is not supported\n");

	struct v4l2_format fmt;
	memset(&fmt, 0, sizeof fmt);
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	ret = ioctl(v4lfd, VIDIOC_G_FMT, &fmt);
	BYE_ON(ret < 0, "VIDIOC_G_FMT failed: %s\n", ERRSTR);
	printf("G_FMT(start): width = %u, height = %u, 4cc = %.4s\n",
		fmt.fmt.pix.width, fmt.fmt.pix.height,
		(char*)&fmt.fmt.pix.pixelformat);

	if (s.use_wh) {
		fmt.fmt.pix.width = s.w;
		fmt.fmt.pix.height = s.h;
	}
	if (s.in_fourcc)
		fmt.fmt.pix.pixelformat = s.in_fourcc;

	ret = ioctl(v4lfd, VIDIOC_S_FMT, &fmt);
	BYE_ON(ret < 0, "VIDIOC_S_FMT failed: %s\n", ERRSTR);

	ret = ioctl(v4lfd, VIDIOC_G_FMT, &fmt);
	BYE_ON(ret < 0, "VIDIOC_G_FMT failed: %s\n", ERRSTR);
	printf("G_FMT(final): width = %u, height = %u, 4cc = %.4s\n",
		fmt.fmt.pix.width, fmt.fmt.pix.height,
		(char*)&fmt.fmt.pix.pixelformat);

	struct v4l2_requestbuffers rqbufs;
	memset(&rqbufs, 0, sizeof(rqbufs));
	rqbufs.count = s.buffer_count;
	rqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	rqbufs.memory = V4L2_MEMORY_DMABUF;

	ret = ioctl(v4lfd, VIDIOC_REQBUFS, &rqbufs);
	BYE_ON(ret < 0, "VIDIOC_REQBUFS failed: %s\n", ERRSTR);
	BYE_ON(rqbufs.count < s.buffer_count, "video node allocated only "
		"%u of %u buffers\n", rqbufs.count, s.buffer_count);

	s.in_fourcc = fmt.fmt.pix.pixelformat;
	s.w = fmt.fmt.pix.width;
	s.h = fmt.fmt.pix.height;

	/* TODO: add support for multiplanar formats */
	struct buffer buffer[s.buffer_count];
	uint32_t size = fmt.fmt.pix.sizeimage;
	uint32_t pitch = fmt.fmt.pix.bytesperline;
	printf("size = %u pitch = %u s.in_fourcc[%.4s]\n", size, pitch, c_fmt(s.in_fourcc));
	for (unsigned int i = 0; i < s.buffer_count; ++i) {
		ret = buffer_create(&buffer[i], drmfd, &s, size, pitch);
		BYE_ON(ret, "failed to create buffer%d\n", i);
	}
	printf("buffers ready\n");

	uint32_t con;
	ret = find_crtc(drmfd, &s, &con);
	BYE_ON(ret, "failed to find valid mode\n");

	ret = find_plane(drmfd, &s);
	BYE_ON(ret, "failed to find compatible plane\n");

	for (unsigned int i = 0; i < s.buffer_count; ++i) {
		struct v4l2_buffer buf;
		memset(&buf, 0, sizeof buf);

		buf.index = i;
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_DMABUF;
		buf.m.fd = buffer[i].dbuf_fd;
		ret = ioctl(v4lfd, VIDIOC_QBUF, &buf);
		BYE_ON(ret < 0, "VIDIOC_QBUF for buffer %d failed: %s (fd %u)\n",
			buf.index, ERRSTR, buffer[i].dbuf_fd);
	}

	int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ret = ioctl(v4lfd, VIDIOC_STREAMON, &type);
	BYE_ON(ret < 0, "STREAMON failed: %s\n", ERRSTR);

	struct pollfd fds[] = {
		{ .fd = v4lfd, .events = POLLIN },
		{ .fd = drmfd, .events = POLLIN },
	};

	/* buffer currently used by drm */
	stream.v4lfd = v4lfd;
	stream.current_buffer = -1;
	stream.buffer = buffer;

	while ((ret = poll(fds, 2, 5000)) > 0) {
		struct v4l2_buffer buf;

                printf("loop ... buf[%d]\n", stream.current_buffer);

		/* dequeue buffer */
		memset(&buf, 0, sizeof buf);
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_DMABUF;
		ret = ioctl(v4lfd, VIDIOC_DQBUF, &buf);
		BYE_ON(ret, "VIDIOC_DQBUF failed: %s\n", ERRSTR);

		ret = drmModeSetPlane(drmfd, s.planeId, s.crtcId,
				      buffer[buf.index].fb_handle, 0,
				      s.compose.left, s.compose.top,
				      s.compose.width,
				      s.compose.height,
				      0, 0, s.w << 16, s.h << 16);
		BYE_ON(ret, "drmModeSetPlane failed: %s\n", ERRSTR);

		if (stream.current_buffer != -1) {
			memset(&buf, 0, sizeof buf);
			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
			buf.memory = V4L2_MEMORY_DMABUF;
			buf.index = stream.current_buffer;
			buf.m.fd = stream.buffer[stream.current_buffer].dbuf_fd;

			ret = ioctl(stream.v4lfd, VIDIOC_QBUF, &buf);
			BYE_ON(ret, "VIDIOC_QBUF(index = %d) failed: %s\n",
			       stream.current_buffer, ERRSTR);
		}

		stream.current_buffer = buf.index;
	}

	return 0;
}

/*
 * Enhanced High Performance Video Encoder driver
 *
 * Copyright 2020 Raritan Inc.
 *
 * 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 of the License.
 */

#include <linux/cdev.h>
#include <linux/delay.h>
#include <linux/dma-mapping.h>
#include <linux/firmware.h>
#include <linux/fs.h>
#include <linux/idr.h>
#include <linux/interrupt.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/of_address.h>
#include <linux/of_device.h>
#include <linux/of_irq.h>
#include <linux/of_reserved_mem.h>
#include <linux/platform_device.h>
#include <linux/poll.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/uaccess.h>
#include <linux/wait.h>
#include <linux/bcd.h>
#include <linux/mutex.h>
#include <linux/pci.h>
#include <linux/pci_ids.h>
#include <linux/compat.h>
#include <linux/raritan/lpvs_ioctl.h>

#include <media/v4l2-common.h>
#include <media/v4l2-ctrls.h>
#include <media/v4l2-event.h>
#include <media/v4l2-fwnode.h>
#include <linux/v4l2-mediabus.h>
#include <media/v4l2-subdev.h>

#define ALLOW_REQUEST_IRQ_ERROR

#define PCI_DEVICE_ID_ALTERA		0x0011

#define PCI_BAR_COUNT			1

#define LPVS_DRIVER_NAME		"LPVS"
#define LPVS_DEVICE_NAME		"lpvs"
#define LPVS_MAX_DEVICES                1

/* driver version */
#define LPVS_MAJOR_NUM			0
#define LPVS_MINOR_NUM			0
#define LPVS_PATCH_NUM			0

/* Top Level Memory Map
 *
 * Register Block                      PCI BAR     Allocated Address Space
 * -------------------------------------------------------------------------
 * Global Registers                       0       0x0000_0000 - 0x0000_003F
 *
 * NOTE: Register addressing is byte size
 */
#define GLOBAL_REGBLK_BASE                  0x00000000

/* Global Registers */
#define LPVS_DEVICE_VERSION                (GLOBAL_REGBLK_BASE + 0x0)
#define LPVS_DEVICE_INFO                   (GLOBAL_REGBLK_BASE + 0x4)
#define LPVS_GLOBAL_INTERRUPT_STATUS       (GLOBAL_REGBLK_BASE + 0x8)
#define LPVS_GLOBAL_INTERRUPT_MASK         (GLOBAL_REGBLK_BASE + 0xc)
#define LPVS_CHANNEL_RESET                 (GLOBAL_REGBLK_BASE + 0x10)
#define LPVS_UNIQUE_CHIP_ID_HIGH           (GLOBAL_REGBLK_BASE + 0x28)
#define LPVS_UNIQUE_CHIP_ID_LOW            (GLOBAL_REGBLK_BASE + 0x2c)
#define LPVS_DIE_TEMPERATURE               (GLOBAL_REGBLK_BASE + 0x30)
#define LPVS_FAN_STATUS                    (GLOBAL_REGBLK_BASE + 0x34)

/*----------------------------------*/
/* Version Register Bit Definitions */
/*----------------------------------*/
#define VR_MINOR                            0x0000FFFF
#define VR_MAJOR                            0xFFFF0000
#define DIR_ID                              0xc0000000

typedef struct {
	struct device *parent;

	u32 id;
	lpvs_info_t info;

	struct pci_dev *pcidev;
	int irq;
	void __iomem *global_reg_base;
	phys_addr_t ioaddr;
	unsigned long iolen;

        atomic_t mode_changed;
        wait_queue_head_t wait;

        struct v4l2_subdev              subdev;
} lpvs_device_t;

static dev_t g_lpvs_devno;
static lpvs_device_t *g_lpvsdev;

static int lpvs_open(struct inode *inode, struct file *file);
static int lpvs_release(struct inode *inode, struct file *file);
static long lpvs_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
#ifdef CONFIG_COMPAT
static long lpvs_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg);
#endif
static unsigned int lpvs_poll(struct file *file, poll_table *wait);


#ifdef CONFIG_COMPAT
static void __user *u64_to_uptr(u64 value)
{
	if (in_compat_syscall())
		return compat_ptr(value);
	else
		return (void __user *)(unsigned long)value;
}
#else
static inline void __user *u64_to_uptr(u64 value)
{
	return (void __user *)(unsigned long)value;
}
#endif /* CONFIG_COMPAT */

static inline u32 lpvs_read_global_reg(lpvs_device_t *dev, u32 reg)
{
	return ioread32(dev->global_reg_base + reg);
}

static inline void lpvs_write_global_reg(lpvs_device_t *dev, u32 reg, u32 val)
{
	iowrite32(val, dev->global_reg_base + reg);
}

/* Start/Stop streaming from the device */
static int lpvs_s_stream(struct v4l2_subdev *sd, int enable)
{
    return 0;
}

static int lpvs_enum_mbus_code(struct v4l2_subdev *sd,
                               struct v4l2_subdev_state *sd_state,
                               struct v4l2_subdev_mbus_code_enum *code)
{
    if (code->index >= 1) return -EINVAL;

    code->code = MEDIA_BUS_FMT_RGB888_1X24;
    return 0;
}

static int lpvs_enum_frame_sizes(struct v4l2_subdev *sd,
                                 struct v4l2_subdev_state *sd_state,
                                 struct v4l2_subdev_frame_size_enum *fse)
{
    if (fse->index >= 1) return -EINVAL;

    fse->code = MEDIA_BUS_FMT_RGB888_1X24;
    //TODO: get width and height (detected or pre-patten)
    u16 width = 1920, height = 1080;
    fse->min_width = width;
    fse->max_width = width;
    fse->max_height = height;
    fse->min_height = height;
    return 0;
}

static int lpvs_get_fmt(struct v4l2_subdev *sd,
                           struct v4l2_subdev_state *sd_state,
                           struct v4l2_subdev_format *fmt)
{
    struct v4l2_mbus_framefmt *mf, lmf;

    pr_info("lpvs_get_fmt() is called: fmt->which[%d]\n", fmt->which);

    if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
        mf = v4l2_subdev_get_try_format(sd, sd_state, 0);
        fmt->format = *mf;
        return 0;
    }

    // TODO: get resolution from AD
    memset(&lmf, 0, sizeof(lmf));
    lmf.width = 1920;
    lmf.height = 1080;
    lmf.colorspace = V4L2_COLORSPACE_SRGB;
    lmf.code = MEDIA_BUS_FMT_RGB888_1X24;
    lmf.field = V4L2_FIELD_NONE;
    fmt->format = lmf;

    return 0;
}

static int lpvs_set_fmt(struct v4l2_subdev *sd,
                           struct v4l2_subdev_state *sd_state,
                           struct v4l2_subdev_format *fmt)
{
    struct v4l2_mbus_framefmt *mf = &fmt->format;

    pr_info("lpvs_set_fmt() is called: mf->code[%d]\n", mf->code);

    if (mf->code != MEDIA_BUS_FMT_RGB888_1X24 || mf->width != 1920 || mf->height != 1080) {
        return -EINVAL;
    }

    return 0;
}

static const struct v4l2_subdev_video_ops lpvs_subdev_video_ops = {
        .s_stream       = lpvs_s_stream,
};

static const struct v4l2_subdev_core_ops lpvs_subdev_core_ops = {
        .log_status             = v4l2_ctrl_subdev_log_status,
        .subscribe_event        = v4l2_ctrl_subdev_subscribe_event,
        .unsubscribe_event      = v4l2_event_subdev_unsubscribe,
};

static const struct v4l2_subdev_pad_ops lpvs_subdev_pad_ops = {
        .enum_mbus_code         = lpvs_enum_mbus_code,
        .enum_frame_size        = lpvs_enum_frame_sizes,
        .get_fmt                = lpvs_get_fmt,
        .set_fmt                = lpvs_set_fmt,
};

static struct v4l2_subdev_ops lpvs_subdev_ops = {
        .core   = &lpvs_subdev_core_ops,
        .video  = &lpvs_subdev_video_ops,
        .pad    = &lpvs_subdev_pad_ops,
};

static const struct file_operations lpvs_fops = {
	.owner		= THIS_MODULE,
	.unlocked_ioctl	= lpvs_ioctl,
#ifdef CONFIG_COMPAT
	.compat_ioctl	= lpvs_compat_ioctl,
#endif
	.open		= lpvs_open,
	.release	= lpvs_release,
	.poll		= lpvs_poll
};

static int 
lpvs_open(struct inode *inode, struct file *file)
{	
	file->f_op = &lpvs_fops;
	file->private_data = g_lpvsdev;
	try_module_get(THIS_MODULE);

	return 0;
}

static int
lpvs_release(struct inode *inode, struct file *file)
{
	module_put(THIS_MODULE);
	return 0;
}

static unsigned int
lpvs_poll(struct file *file, poll_table *wait)
{
	lpvs_device_t *dev = (lpvs_device_t *)file->private_data;

	poll_wait(file, &dev->wait, wait);

	if (atomic_read(&dev->mode_changed)) {
		atomic_set(&dev->mode_changed, 0);
		return POLLIN;
	}

	return 0;
}

static long
lpvs_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
	lpvs_device_t *dev = (lpvs_device_t *)file->private_data;
	void __user *argp = (void __user *)arg;
	lpvs_msg_t *msg = NULL;
	int size = _IOC_SIZE( cmd );
	int rc = 0;

	if (_IOC_TYPE(cmd) != LPVS_MAGIC) return -ENOTTY;
        
	if (dev == NULL) {
		printk("(%s) NULL dev\n", __FUNCTION__);
		return -ENOTTY;
	}
        
	/* verify the structure of the command */
	if (size > sizeof(lpvs_msg_t))
		return -ENOTTY;

	/* kernel crash workaround: dynamically allocate structure */
	if (size > 0) {/* verify the structure of the command */
		msg = kmalloc(sizeof(lpvs_msg_t), GFP_KERNEL);
		if (!msg) {
			dev_err(&dev->pcidev->dev, "lpvs_msg_t not allocated\n");
			return -ENOMEM;
		}
	}

	/* Copy the data from the user space */
	if (_IOC_DIR(cmd) & _IOC_WRITE) {
		if (copy_from_user(msg, argp, size)) {
			dev_err(&dev->pcidev->dev, "%s: Copy from user space %p %p %d failed\n", __FUNCTION__, msg, argp, size );
			rc = -EFAULT;
			goto ioctl_cleanup;
		}
	}

	switch (cmd) {
                /* TODO */
		default:
			dev_err(&dev->pcidev->dev, "ERROR: Invalid ioctl cmd %x (%d)\n", cmd, _IOC_NR(cmd));
			rc = -EPERM;
	}

	/* copy the data back to the user space */
	if (rc == 0 && (_IOC_DIR(cmd) & _IOC_READ)) {
		if (copy_to_user(argp, msg, size)) {
			dev_err(&dev->pcidev->dev, "%s: Copy to user space %p %p %d failed\n", __FUNCTION__, msg, argp, size);
			rc = -EFAULT;
		}
	}

ioctl_cleanup:
	if (msg) {
		kfree(msg);
	}

	return rc;
}

#ifdef CONFIG_COMPAT
static long
lpvs_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
	switch (_IOC_NR(cmd)) {
		case _IOC_NR(LPVS_READ_SDRAM):
		case _IOC_NR(LPVS_WRITE_SDRAM):
			if (_IOC_SIZE(cmd) != sizeof(lpvs_rw_sdram_t)) {
				/* fix up pointer size */
				cmd &= ~IOCSIZE_MASK;
				cmd |= sizeof(lpvs_rw_sdram_t) << IOCSIZE_SHIFT;
			}
			break;
		case _IOC_NR(LPVS_WRITE_DDC):
			if (_IOC_SIZE(cmd) != sizeof(lpvs_ddc_t)) {
				/* fix up pointer size */
				cmd &= ~IOCSIZE_MASK;
				cmd |= sizeof(lpvs_ddc_t) << IOCSIZE_SHIFT;
			}
			break;
		default:
			break;
	}

	return lpvs_ioctl(file, cmd, (unsigned long)compat_ptr(arg));
}
#endif

#if 1
static irqreturn_t lpvs_isr(int irq, void *dev_arg)
{
	lpvs_device_t *dev = dev_arg;
	u_int32_t gisr;
	int handled = 0;

	/* read Global Interrupt Status to determine module that requires processing */
	gisr = lpvs_read_global_reg(dev, LPVS_GLOBAL_INTERRUPT_STATUS);
	if (!gisr) {
		/* interrupt is not for us */
		return IRQ_NONE;
	}
	dev_info(dev->parent, "gisr 0x%08x\n", gisr);
        
	/* work-around to missing interrupt when using msi */
	lpvs_write_global_reg(dev, LPVS_GLOBAL_INTERRUPT_STATUS, gisr);

	return IRQ_RETVAL(handled);
}
#endif // if 0

static int
lpvs_probe(struct pci_dev *pcidev, const struct pci_device_id *ent)
{
	struct device *dev = &pcidev->dev;
        struct device_node *np;
	lpvs_device_t *info;
        struct v4l2_subdev *sd;
	int rc;
	u32 reg;
	int got_pci_req_region = 0;
	int irq_have_been_setup = 0;
	int irq_flag;

	if (pci_enable_device(pcidev) < 0) {
		return -ENODEV;
	}

	info = kzalloc(sizeof(*info), GFP_KERNEL);
	if (!info) {
		dev_err(dev, "Couldn't allocate device private record\n");
		return -ENOMEM;
	}

	/* Preference it to use MSI/MSI-X interrupts */
	irq_flag = PCI_IRQ_MSI | PCI_IRQ_MSIX;

	rc = pci_alloc_irq_vectors(pcidev, 1, 32, irq_flag);
	if (rc < 0) {
		dev_info(dev, "No msi irq vector found\n");

		/* try to use PCI legacy IRQ instead */
		rc = pci_alloc_irq_vectors(pcidev, 1, 32, PCI_IRQ_LEGACY);
		if (rc < 0) {
			dev_err(dev, "Failed to allocate irq vector\n");
			return rc;
		}
	}

	dev_dbg(dev, "#irq_vector %d, msi %s, msix %s\n", rc,
		pcidev->msi_enabled ? "enabled" : "disabled",
		pcidev->msix_enabled ? "enabled" : "disabled");

	info->pcidev = pcidev;
	info->irq = pci_irq_vector(pcidev, 0);
	info->ioaddr = pci_resource_start(pcidev, 0);
	info->iolen = pci_resource_len(pcidev, 0);

	dev_info(dev, "LPVS FPGA device found at %02x:%02x\n",
                pcidev->bus->number, pcidev->devfn);

	if (pci_request_regions(pcidev, LPVS_DRIVER_NAME)) {
	    dev_err(dev, "lpvs%d: Unable to request memory region\n",
		    info->id);
	    rc = -ENOMEM;
	    goto probe_error;
	}
	got_pci_req_region = 1;

	info->global_reg_base = pci_iomap(pcidev, 0, 0);

#if defined(CONFIG_64BIT)
	dev_info(dev, "lpvs: bar 0x%llx -> 0x%px (0x%lx)\n",
		 info->ioaddr, info->global_reg_base,
		 info->iolen);
#else
	dev_info(dev, "lpvs: bar 0x%x -> 0x%px (0x%lx)\n",
		 info->ioaddr, info->global_reg_base,
		 info->iolen);
#endif

        /* find of node */
        np = of_find_node_by_path("/lpvs");
        if (!np) {
            dev_err(dev, "missing lpvs node in device tree\n");
            rc = -ENOMEM;
            goto probe_error;
        }
        dev->of_node = np;

        sd = &info->subdev;
        v4l2_subdev_init(sd, &lpvs_subdev_ops);
        sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
                     V4L2_SUBDEV_FL_HAS_EVENTS;
        sd->dev = dev;
        snprintf(sd->name, sizeof(sd->name), "%s-subdev", LPVS_DEVICE_NAME);

	rc = request_irq(info->irq, lpvs_isr, IRQF_SHARED, "LPVS", info);
	if (rc < 0) {
		dev_err(dev, "Can't install LPVS IRQ %d (rc=%d)\n",
			info->irq, rc);
#ifndef ALLOW_REQUEST_IRQ_ERROR
		goto probe_error;
#endif
	} else {
		irq_have_been_setup = 1;
	}

	/* retrieve version numbers */
	reg = lpvs_read_global_reg(info, LPVS_DEVICE_VERSION);
	dev_info(dev, "LPVS Device Version: %08x\n", reg);
	/* major is hardware verison (0=eval, 1=proto, 2=ES1) */
	info->info.fpga_version.major = (reg & VR_MAJOR) >> 16;
	/* minor is versioning of chip-wide stuff */
	info->info.fpga_version.minor = reg & VR_MINOR;
	info->info.fpga_version.patch = 0;
	info->info.driver_version.major = LPVS_MAJOR_NUM;
	info->info.driver_version.minor = LPVS_MINOR_NUM;
	info->info.driver_version.patch = LPVS_PATCH_NUM;
	info->info.chip_id_high = lpvs_read_global_reg(info, LPVS_UNIQUE_CHIP_ID_HIGH);
	info->info.chip_id_low = lpvs_read_global_reg(info, LPVS_UNIQUE_CHIP_ID_LOW);
	/* get video channel count from fpga */
	reg = lpvs_read_global_reg(info, LPVS_DEVICE_INFO);
	dev_info(dev, "LPVS Device Info: %08x\n", reg);

	/* configure device for PCI bus master */
	pci_write_config_word(pcidev, PCI_COMMAND, 0x0146);

	info->parent = get_device(&pcidev->dev);
	dev_set_drvdata(dev, info);

	g_lpvsdev = info;
	dev_info(dev, "+++ FPGA Device Ver %d.%d.%d "
		 "Driver Ver %d.%d.%d\n",
		 info->info.fpga_version.major,
		 info->info.fpga_version.minor,
		 info->info.fpga_version.patch,
		 info->info.driver_version.major,
		 info->info.driver_version.minor,
		 info->info.driver_version.patch);

	/* enable global interrupt */
	lpvs_write_global_reg(info, LPVS_GLOBAL_INTERRUPT_MASK, 0x0);

#if 1
        rc = v4l2_async_register_subdev(sd);
        if (rc) {
		dev_err(dev, "Cannot async register v4l2 subdev (rc=%d)\n", rc);
        }
#endif

        return 0;

probe_error:
	if (info) {
		if (irq_have_been_setup) {
			free_irq(info->irq, info);
		}

		if (info->global_reg_base) {
			pci_iounmap(pcidev, info->global_reg_base);
		}

		if (got_pci_req_region) {
			pci_release_regions(pcidev);
		}

		pci_disable_device(pcidev);

		kfree(info);
	}

	return rc;
}

static void lpvs_remove(struct pci_dev *pdev)
{
	lpvs_device_t *info = (lpvs_device_t *)pci_get_drvdata(pdev);

	free_irq(info->irq, info);

	pci_iounmap(pdev, info->global_reg_base);
	pci_release_regions(pdev);
	pci_disable_device(pdev);

	kfree(info);
}

static struct pci_device_id lpvs_pci_tbl[] = {
    { PCI_VENDOR_ID_ALTERA, PCI_DEVICE_ID_ALTERA, PCI_ANY_ID, PCI_ANY_ID, },
    { 0, },
};
MODULE_DEVICE_TABLE(pci, lpvs_pci_tbl);

static struct pci_driver lpvs_driver = {
	.name = LPVS_DRIVER_NAME,
	.id_table = lpvs_pci_tbl,
	.probe = lpvs_probe,
	.remove	= lpvs_remove,
};

static int __init lpvs_init(void)
{
	int rc;

	rc = alloc_chrdev_region(&g_lpvs_devno, 0, 1, LPVS_DEVICE_NAME);
	if (rc) {
		return rc;
	}

	rc = pci_register_driver(&lpvs_driver);
	if (rc < 0) {
		printk("Registering LPVS PCI driver failed (rc=%d)\n", rc);
		unregister_chrdev_region(g_lpvs_devno, 1);
		return rc;
	}

	return 0;
}

static void __exit lpvs_exit(void)
{
	pci_unregister_driver(&lpvs_driver);
	unregister_chrdev_region(g_lpvs_devno, 1);
}

module_init(lpvs_init);
module_exit(lpvs_exit);

MODULE_AUTHOR("Yong Zou");
MODULE_DESCRIPTION("Raritan Local Port Video Sync driver");
MODULE_LICENSE("GPL");
MODULE_VERSION("1.00");