Mity CPU Platforms » MityDSP-L138 (ARM9 Based Platforms)

#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;

}