#include "usbctrl.h"
#include "DspParserBase.h"
#include "ReportError.h"

#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <memory.h>
#include <pthread.h>
#include <signal.h>
#include <stdlib.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/poll.h>
#include <stdint.h>

#include <sys/syscall.h>
#include <linux/aio_abi.h>

using namespace MityDSP;

extern "C" {

#include <asm/byteorder.h>

#define le16_to_cpu __le16_to_cpu

#include <linux/types.h>
#include <linux/usb/gadgetfs.h>
#include <linux/usb/ch9.h>

#include "usbstring.h"

#define DRIVER_VENDOR_NUM	0x1EF8 /* Critical Link */
#define DRIVER_PRODUCT_NUM	0x1005 /* MityCAM */
#define	STRINGID_MFGR		1
#define	STRINGID_PRODUCT	2
#define	STRINGID_SERIAL		3
#define	STRINGID_CONFIG		4
#define	STRINGID_INTERFACE	5

#define USB_BUFSIZE (5*1024)

static struct usb_device_descriptor device_desc = {
	bLength			: sizeof(device_desc),
	bDescriptorType		: USB_DT_DEVICE,
	bcdUSB			: __constant_cpu_to_le16(0x0200),
	bDeviceClass		: USB_CLASS_VENDOR_SPEC,
	bDeviceSubClass		: 0,
	bDeviceProtocol		: 0,
	bMaxPacketSize0		: 0,
	idVendor		: __constant_cpu_to_le16(DRIVER_VENDOR_NUM),
	idProduct		: __constant_cpu_to_le16(DRIVER_PRODUCT_NUM),
	bcdDevice		: __constant_cpu_to_le16(0x0107),
	iManufacturer		: STRINGID_MFGR,
	iProduct		: STRINGID_PRODUCT,
	iSerialNumber		: STRINGID_SERIAL,
	bNumConfigurations	: 1,
};

#define MAX_USB_POWER	1

static const struct usb_config_descriptor config = {
	bLength			: sizeof(config),
	bDescriptorType 	: USB_DT_CONFIG,
	wTotalLength		: 0,
	bNumInterfaces		: 1,
	bConfigurationValue	: 3,
	iConfiguration		: 0,
	bmAttributes		: (USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER),
	bMaxPower		: ((MAX_USB_POWER + 1) / 2),
};

static const struct usb_interface_descriptor bulk_intf = {
	bLength			: sizeof(bulk_intf),
	bDescriptorType		: USB_DT_INTERFACE,
	bInterfaceNumber	: 0,
	bAlternateSetting	: 0,
	bNumEndpoints		: 2,
	bInterfaceClass		: USB_CLASS_VENDOR_SPEC,
	bInterfaceSubClass	: 0,
	bInterfaceProtocol	: 0,
	iInterface		: STRINGID_INTERFACE,
};

static struct usb_endpoint_descriptor fs_source_desc = {
	bLength			: USB_DT_ENDPOINT_SIZE,
	bDescriptorType		: USB_DT_ENDPOINT,
	bEndpointAddress	: USB_DIR_IN | 1,
	bmAttributes		: USB_ENDPOINT_XFER_BULK,
	wMaxPacketSize		: __constant_cpu_to_le16(64),
	bInterval		: 0,
	bRefresh		: 0,
	bSynchAddress		: 0,
};

static struct usb_endpoint_descriptor fs_sink_desc = {
	bLength			: USB_DT_ENDPOINT_SIZE,
	bDescriptorType		: USB_DT_ENDPOINT,
	bEndpointAddress	: USB_DIR_OUT | 2,
	bmAttributes		: USB_ENDPOINT_XFER_BULK,
	wMaxPacketSize		: __constant_cpu_to_le16(64),
	bInterval		: 0,
	bRefresh		: 0,
	bSynchAddress		: 0,
};

static const struct usb_endpoint_descriptor *fs_eps[2] = {
	&fs_source_desc,
	&fs_sink_desc,
};

static struct usb_endpoint_descriptor hs_source_desc = {
	bLength			: USB_DT_ENDPOINT_SIZE,
	bDescriptorType		: USB_DT_ENDPOINT,
	bEndpointAddress	: USB_DIR_IN | 1,
	bmAttributes		: USB_ENDPOINT_XFER_BULK,
	wMaxPacketSize		: __constant_cpu_to_le16(512),
	bInterval		: 0,
	bRefresh		: 0,
	bSynchAddress		: 0,
};

static struct usb_endpoint_descriptor hs_sink_desc = {
	bLength			: USB_DT_ENDPOINT_SIZE,
	bDescriptorType		: USB_DT_ENDPOINT,
	bEndpointAddress	: USB_DIR_OUT | 2,
	bmAttributes		: USB_ENDPOINT_XFER_BULK,
	wMaxPacketSize		: __constant_cpu_to_le16(512),
	bInterval		: 0,
	bRefresh		: 0,
	bSynchAddress		: 0,
};

static const struct usb_endpoint_descriptor *hs_eps[2] = {
	&hs_source_desc,
	&hs_sink_desc,
};

static char serial[64];

static struct usb_string stringtab[] = {
	{ STRINGID_MFGR,	"Critical Link, LLC", },
	{ STRINGID_PRODUCT,	"MityCAM", },
	{ STRINGID_SERIAL,	serial, },
	{ STRINGID_CONFIG,	"Config", },
	{ STRINGID_INTERFACE,	"Bulk Transfer", },
};

static struct usb_gadget_strings strings = {
	language	: 0x0409, /* en-us */
	strings		: stringtab,
};

}

/**
 *  Gadget FS Based USB peripheral driver code.
 *
 *  \param[in] apParser pointer to valid parser for data handling
 *  \param[in] anSerial the camera serial number
 *  \param[in] apRe pointer to valid report error class
 */
tcUSB::tcUSB(tcDspParserBase* apParser, int anSerial, tcReportError* apRe)
: mnEP0Fd(-1)
, mnBulkInFd(-1)
, mnBulkOutFd(-1)
, mpParser(apParser)
, mhEP0Thread(pthread_self())
, mhBulkThread(pthread_self())
, mhStartupThread(pthread_self())
, mnBytesIn(0)
, mpReportError(apRe)
, mbConnected(false)
{
	sprintf(serial, "%d", anSerial);
}

/**
 *  Interface destructor 
 */
tcUSB::~tcUSB(void)
{
	if (mnEP0Fd >= 0)
		close(mnEP0Fd);
}

char* tcUSB::build_config(char* cp,
	const struct usb_endpoint_descriptor **ep)
{
	struct usb_config_descriptor *c;
	int i;

	c = (struct usb_config_descriptor *)cp;

	memcpy(cp, &config, config.bLength);
	cp += config.bLength;

	memcpy(cp, &bulk_intf, bulk_intf.bLength);
	cp += bulk_intf.bLength;

	for (i = 0; i < bulk_intf.bNumEndpoints; i++)
	{
		memcpy(cp, ep[i], USB_DT_ENDPOINT_SIZE);
		cp += USB_DT_ENDPOINT_SIZE;
	}
	c->wTotalLength = __cpu_to_le16(cp - (char*)c);
	return cp;
}

int tcUSB::Initialize(void)
{
	char buf[4096], *cp = &buf[0];
	int rv, status;

	mnEP0Fd = open("/dev/gadgetfs/musb-hdrc", O_RDWR);
	if (mnEP0Fd < 0)
	{
		return mnEP0Fd;
	}
	mpReportError->Report("usb device opened\n");

	*(__u32 *)cp = 0;
	cp += 4;

	/* write full speed configs */
	cp = build_config(cp, fs_eps);

	/* write high speed configs */
	cp = build_config(cp, hs_eps);

	/* write device descriptor */
	memcpy(cp, &device_desc, sizeof(device_desc));
	cp += sizeof(device_desc);

	rv = write(mnEP0Fd, &buf[0], cp - buf);
	if (rv < 0)
	{
		mpReportError->Report("initial write error : %s\n", strerror(errno));
		close(mnEP0Fd);
		mnEP0Fd = -1;
		return mnEP0Fd;
	}
	else if (rv != (cp - buf))
	{
		mpReportError->Report("rv mismatch : %d, %d\n", rv, (cp-buf));
		close(mnEP0Fd);
		mnEP0Fd = -2;
		return mnEP0Fd;
	}


	/* spawn the control endpoint reader thread */
	pthread_attr_t attr;
	rv = pthread_attr_init(&attr);

	status = pthread_create(&mhEP0Thread, &attr, DispatchEP0Handler, (void*)this);
	if (status)
	{
		mpReportError->Report("pthread_create (network poll thread) : %s\n", strerror(errno));
		return status;
	}

	return 0;
}

int tcUSB::StopIO(void)
{
	if (!pthread_equal(mhBulkThread, mhStartupThread))
	{
		pthread_cancel(mhBulkThread);
		if (pthread_join(mhBulkThread, 0) != 0)
		{
			mpReportError->Report("Could not stop Bulk Thread");
		}
		else
		{
			close(mnBulkOutFd);
			mnBulkOutFd = -1;
		}
		mhBulkThread = mhStartupThread;
	}
	if (mnBulkInFd >= 0)
	{
		close(mnBulkInFd);
		mnBulkInFd = -1;
	}
	return 0;
}

int tcUSB::StartIO(void)
{
	int status, rv;
	uint32_t bufi[256/4];
	char* buf = (char*)&bufi[0];
	pthread_attr_t attr;

	rv = pthread_attr_init(&attr);

	/* open up the bulk input endpoint (relative to PC)
	 * file descriptor for sending data via put() call */
	mnBulkInFd = open("/dev/gadgetfs/ep1in", O_WRONLY);
	if (mnBulkInFd < 0) 
	{
		mpReportError->Report("Could not open ep1in (%s)\n",
			strerror(errno));
		return -errno;
	}

	/* write full speed descriptor in */
	*(uint32_t *)buf = 1;
	memcpy(buf + 4, &fs_source_desc, USB_DT_ENDPOINT_SIZE);

	/* write high speed descriptor in */
	memcpy(buf + 4 + USB_DT_ENDPOINT_SIZE,
		&hs_source_desc, USB_DT_ENDPOINT_SIZE);

	/* push it to control framework */
	if (write(mnBulkInFd, buf, 4 + 2 * USB_DT_ENDPOINT_SIZE) < 0)
	{
		mpReportError->Report("Could not setup bulkin descriptors "
			" (%s)\n", strerror(errno));
		close(mnBulkInFd);
		return -errno;
	}

	/* open up the bulk output endpoint (relative to PC)
	 * file descriptor for receiving app data */
	mnBulkOutFd = open("/dev/gadgetfs/ep2out", O_RDWR);
	if (mnBulkOutFd < 0)
	{
		mpReportError->Report("Could not open ep2out (%s)\n",
			strerror(errno));
		close(mnBulkInFd);
		return -errno;
	}

	/* write full speed descriptor in */
	*(uint32_t *)buf = 1;
	memcpy(buf + 4, &fs_sink_desc, USB_DT_ENDPOINT_SIZE);

	/* write high speed descriptor in */
	memcpy(buf + 4 + USB_DT_ENDPOINT_SIZE,
		&hs_sink_desc, USB_DT_ENDPOINT_SIZE);

	/* push it to control framework */
	if (write(mnBulkOutFd, buf, 4 + 2 * USB_DT_ENDPOINT_SIZE) < 0)
	{
		mpReportError->Report("Could not setup bulkout descriptors "
			" (%s)\n", strerror(errno));
		close(mnBulkInFd);
		close(mnBulkOutFd);
		return -errno;
	}

	/* spawn the bulk endpoint reader thread */
	status = pthread_create(&mhBulkThread, &attr, DispatchBulkInHandler, (void*)this);
	if (status)
	{
		mpReportError->Report("pthread_create (usb poll thread) : %s\n", strerror(errno));
	}

	return status;
}

void* tcUSB::DispatchEP0Handler(void *apThis)
{
	tcUSB *lpUSB = (tcUSB *)apThis;
	lpUSB->EP0Handler();
	return NULL;
}

#define USB_REQ_GET_STATE	0xDF
#define USB_REQ_CLEAR_INKTEND	0xDF
#define USB_REQ_PEEK_BUFF	0xDD
#define USB_REQ_POKE_BUFF	0xDC
#define USB_REQ_WRITE_SN	0xDB
#define USB_REQ_READ_SN		0xDA

int tcUSB::HandleControl(struct usb_ctrlrequest *setup)
{
	uint16_t value, index, length;
	uint8_t buf[256];
	int status, tmp;

	value = __le16_to_cpu(setup->wValue);
	index = __le16_to_cpu(setup->wIndex);
	length = __le16_to_cpu(setup->wLength);

#if 0
	printf("Handle Control : %02x.%02x v%04x i%04x %d\n",
			setup->bRequestType, setup->bRequest,
			value, index, length);
#endif
	
	switch(setup->bRequest)
	{
	case USB_REQ_GET_DESCRIPTOR:
		switch(value >> 8)
		{
		case USB_DT_STRING:
			tmp = value & 0xFF;
			if (tmp || index != strings.language)
				goto stall;
			status = usb_gadget_get_string(&strings, tmp, buf);
			if (status < 0)
				goto stall;
			tmp = status;
			if (length < tmp)
				tmp = length;
			write(mnEP0Fd, buf, tmp);
			break;
		default:
			goto stall;
		}
		return 0;

	case USB_REQ_SET_CONFIGURATION:
		if (setup->bRequestType != USB_DIR_OUT)
			goto stall;
		if (value)
		{
			StartIO();
		}
		else
		{
			StopIO();
		}
		status = read(mnEP0Fd, &status, 0);
		if (status)
			mpReportError->Report("ack SET_CONFIGURATION\n");
		return 0;

	case USB_REQ_GET_INTERFACE:
		return 0;

	case USB_REQ_SET_INTERFACE:
		return 0;

	case USB_REQ_GET_STATE:
		for (int i = 0; i < 20; ++i)
		{
			buf[i] = 0x01;
		}
		status = write(mnEP0Fd, buf, 20);
		if (status < 0)
			goto stall;
		return 0;

	case USB_REQ_PEEK_BUFF:
		for (int i = 0; i < 48; ++i)
		{
			buf[i] = 0x00;
		}
		status = write(mnEP0Fd, buf, 48);
		if (status < 0)
			goto stall;
		return 0;

	case USB_REQ_POKE_BUFF:
		status = read(mnEP0Fd, &status, 0);
		return 0;

	default:
		goto stall;
	}

stall:
	if (setup->bRequestType & USB_DIR_IN)
		status = read(mnEP0Fd, &status, 0);
	else
		status = write(mnEP0Fd, &status, 0);

	if (errno != EL2HLT)
		mpReportError->Report("ep0 stall");

	return 0;
}

int tcUSB::EP0Handler(void)
{
	struct pollfd ep0_poll;

	ep0_poll.fd = mnEP0Fd;
	ep0_poll.events = POLLIN | POLLOUT | POLLHUP;

	while (true)
	{
		int	tmp, i, nevent;
		struct usb_gadgetfs_event	event[8];

		/* TODO - infinite timeout? */
		tmp = poll(&ep0_poll, 1, -1);
		if (tmp < 0)
		{
			mpReportError->Report("%s: poll : %s", __func__, strerror(errno));
			break;
		}

		tmp = read(mnEP0Fd, &event, sizeof(event));
		if (tmp < 0)
		{
			if (errno == EAGAIN)
			{
				continue;
			}
			mpReportError->Report("ep0 read after poll : %s\n", strerror(errno));
			goto done;
		}
		nevent = tmp / sizeof(event[0]);
		for(i = 0; i < nevent; ++i)
		{
			switch(event[i].type)
			{
			case GADGETFS_NOP:
				break;
			case GADGETFS_CONNECT:
				mbConnected = true;
				break;
			case GADGETFS_SETUP:
				mbConnected = true;
				HandleControl(&event[i].u.setup);
				break;
			case GADGETFS_DISCONNECT:
				mbConnected = false;
				StopIO();
				break;
			case GADGETFS_SUSPEND:
				break;
			default:
				mpReportError->Report("unhandled USB event %d\n", event[i].type);
				break;
			}
		}
		continue;
done:
		if (mbConnected)
			StopIO();
		break;
	}
	return 0;
}

void* tcUSB::DispatchBulkInHandler(void *apThis)
{
	tcUSB *lpUSB = (tcUSB *)apThis;
	lpUSB->BulkInHandler();
	return NULL;
}

int tcUSB::BulkInHandler(void)
{
	int	status;
	char	buf[USB_BUFSIZE];

	while(true)
	{
		status = read(mnBulkOutFd, buf, sizeof(buf));
		if (status < 0)
		{
			break;
		}
		if (status) {
			mnBytesIn += status;
			mpParser->AddData(buf, status);
		}
	}
	return 0;
}

#define USEAIO

#ifdef USEAIO
inline int io_setup(unsigned nr, aio_context_t *ctxp)
{
	return syscall(__NR_io_setup, nr, ctxp);
}

inline int io_destroy(aio_context_t ctx)
{
	return syscall(__NR_io_destroy, ctx);
}

inline int io_submit(aio_context_t ctx, long nr, struct iocb **iocbpp)
{
	return syscall(__NR_io_submit, ctx, nr, iocbpp);
}

inline int io_getevents(aio_context_t ctx, long min_nr, long max_nr,
			struct io_event *events, struct timespect *timeout)
{
	return syscall(__NR_io_getevents, ctx, min_nr, max_nr, events, timeout);
}

#define NUM_BUFFERS 8
aio_context_t ctx;
struct iocb iocb[NUM_BUFFERS];
static int pingpong = 0;
struct io_event events[NUM_BUFFERS];
char *buffers[NUM_BUFFERS];
#endif

int bufpos = 0;
#define BUFFER_SIZE	(64*1024)
char buffer[BUFFER_SIZE];

/**
 *  Send data via Bulk transfer output endpoint.
 */
int tcUSB::put(const char* data, int length, int flush)
{
	int rv, bytes_sent = 0;
	int index;
	if (length <= 0)
	{
		/* printf("Length was %d\n", length); */
		return 0;
	}
	if (mbConnected && mnBytesIn && (mnBulkInFd >= 0))
	{
#ifndef USEAIO
		if (bufpos + length > BUFFER_SIZE)
		{
			rv = write(mnBulkInFd, buffer, bufpos);
			bufpos = 0;
		}
		memcpy(&buffer[bufpos], data, length);
		bufpos += length;
		if (flush) {
			rv = write(mnBulkInFd, &buffer[0], bufpos);
			bufpos = 0;
		}
#else
		if (0 == pingpong && !bufpos)
		{
			for (int i = 0; i < NUM_BUFFERS; ++i)
				buffers[i] = (char*)malloc(BUFFER_SIZE);

			rv = io_setup(NUM_BUFFERS+1, &ctx);
			if (rv < 0)
			{
				perror("io_setup");
				return -1;
			}
		}
		index = pingpong;
		index %= NUM_BUFFERS;

		int recoverbufs = 0; /* number of buffers we will transfer */
		int first_buf_size = 0;

		/* if we can't fit this packet in our current buffer, advance to next
		 * and note fact by increasing buffer transfer count */
		if (bufpos + length > BUFFER_SIZE)
		{
			recoverbufs++;
			index++;
			index %= NUM_BUFFERS;
			first_buf_size = bufpos;
			bufpos = 0;
		}
		/* if we have to flush, then we must mark this packet to transfer */
		if (flush)
			recoverbufs++;

		/* if we need to transfer and all our buffers are outstanding, then
		 * wait for some to recover */
		if (recoverbufs && (pingpong+recoverbufs > NUM_BUFFERS))
		{
			rv = io_getevents(ctx, recoverbufs, recoverbufs, events, NULL);
			if (rv < 0)
			{
				printf("io_getevents : %d\n", rv);
				return -1;
			}
		}

		/* if two transfers send first packet */
		if (first_buf_size)
		{
			int tmp = index-1;
			if (tmp < 0) tmp = NUM_BUFFERS-1;
			memset(&iocb[tmp],0,sizeof(iocb[tmp]));
			iocb[tmp].aio_fildes = mnBulkInFd;
			iocb[tmp].aio_lio_opcode = IOCB_CMD_PWRITE;
			iocb[tmp].aio_buf = (__u64)buffers[tmp];
			iocb[tmp].aio_nbytes = first_buf_size;
			iocb[tmp].aio_offset = 0;
			struct iocb *cbs[1];
			cbs[0] = &iocb[tmp];
			rv = io_submit(ctx, 1, cbs);
			if (rv != 1)
			{
				perror("io_submit\n");
				return -1;
			}
		}
		/* copy packet data in... */
		memcpy(&buffers[index][bufpos], data, length);
		bufpos += length;

		/* if we need to transfer this packet, send it out */
		if (flush)
		{
			memset(&iocb[index],0,sizeof(iocb[index]));
			iocb[index].aio_fildes = mnBulkInFd;
			iocb[index].aio_lio_opcode = IOCB_CMD_PWRITE;
			iocb[index].aio_buf = (__u64)buffers[index];
			iocb[index].aio_nbytes = bufpos;
			iocb[index].aio_offset = 0;
			struct iocb *cbs[1];
			cbs[0] = &iocb[index];
			bufpos = 0;
			rv = io_submit(ctx, 1, cbs);
			if (rv != 1)
			{
				perror("io_submit\n");
				return -1;
			}
		}

		bytes_sent = length;
		pingpong += recoverbufs;
#endif	
	}
	else
	{
		return -1;
	}
	return bytes_sent;
}