marvell/uboot/drivers/usb/gadget/f_rdp.c

/*
 * f_rdp.c -- USB ramdump driver
 * (C) Copyright 2017 ASR Microelectronics (Shanghai) Co., Ltd.
 * All rights reserved.
 *
 * SPDX-License-Identifier:	GPL-2.0+
 */

#include <errno.h>
#include <common.h>
#include <malloc.h>
#include <version.h>
#include <linux/usb/ch9.h>
#include <linux/usb/gadget.h>
#include <linux/usb/composite.h>
#include <linux/compiler.h>
#include <asm/sizes.h>

#ifdef CONFIG_RAMDUMP
#include <../../../board/Marvell/common/ramdump.h>
#endif

#define DATA_BLOCK_LEN (512 * 7)
#define USB_REG_BUF_LEN	(256)
#define DWC3_USB_REG_BUF_LEN	(512)

struct rd_ram_desc {
	unsigned signature;
	unsigned long textsize;
	unsigned long addr;
	unsigned long datasize;
};

struct rdp_dev {
	struct usb_gadget *gadget;
	struct usb_request *req;
	struct usb_ep *in_ep, *out_ep;
	struct usb_request *in_req, *out_req;
	unsigned char configuration_done;
	unsigned char rxdata;
	unsigned char txdata;
};

struct f_rdp {
	struct usb_function usb_function;
	struct rdp_dev *dev;
};

static struct usb_interface_descriptor rdp_intf_data = {
	.bLength =		sizeof(rdp_intf_data),
	.bDescriptorType =	USB_DT_INTERFACE,
	.bNumEndpoints =	2,
	.bInterfaceClass =	0xff,
	.bInterfaceSubClass =  0xff,
	.bInterfaceProtocol =  0xff,
};

static struct usb_endpoint_descriptor fs_in_desc = {
	.bLength =		USB_DT_ENDPOINT_SIZE,
	.bDescriptorType =	USB_DT_ENDPOINT,

	.bEndpointAddress =	USB_DIR_IN,
	.bmAttributes =	USB_ENDPOINT_XFER_BULK,
};

static struct usb_endpoint_descriptor fs_out_desc = {
	.bLength =		USB_DT_ENDPOINT_SIZE,
	.bDescriptorType =	USB_DT_ENDPOINT,

	.bEndpointAddress =	USB_DIR_OUT,
	.bmAttributes =	USB_ENDPOINT_XFER_BULK,
};

static struct usb_endpoint_descriptor hs_in_desc = {
	.bLength =		USB_DT_ENDPOINT_SIZE,
	.bDescriptorType =	USB_DT_ENDPOINT,

	.bmAttributes =	USB_ENDPOINT_XFER_BULK,
	.wMaxPacketSize =	__constant_cpu_to_le16(512),
};

static struct usb_endpoint_descriptor hs_out_desc = {
	.bLength =		USB_DT_ENDPOINT_SIZE,
	.bDescriptorType =	USB_DT_ENDPOINT,

	.bmAttributes =	USB_ENDPOINT_XFER_BULK,
	.wMaxPacketSize =	__constant_cpu_to_le16(512),
};

static const struct usb_descriptor_header *hs_rdp_data_function[] = {
	(struct usb_descriptor_header *)&rdp_intf_data,
	(struct usb_descriptor_header *)&hs_in_desc,
	(struct usb_descriptor_header *)&hs_out_desc,
	NULL,
};
static const struct usb_descriptor_header *fs_rdp_data_function[] = {
	(struct usb_descriptor_header *)&rdp_intf_data,
	(struct usb_descriptor_header *)&fs_in_desc,
	(struct usb_descriptor_header *)&fs_out_desc,
	NULL,
};

static struct f_rdp *rdp_func;
static inline struct f_rdp *func_to_rdp(struct usb_function *f)
{
	return container_of(f, struct f_rdp, usb_function);
}

static inline struct usb_endpoint_descriptor *
ep_desc(struct usb_gadget *g, struct usb_endpoint_descriptor *hs,
	struct usb_endpoint_descriptor *fs)
{
	printf("dualspeed(g): %d, g->speed: 0x%x\n",
			gadget_is_dualspeed(g), g->speed);

	if (gadget_is_dualspeed(g) && g->speed == USB_SPEED_HIGH)
		return hs;
	return fs;
}

//#define RDP_DEBUG_MODE
#ifdef RDP_DEBUG_MODE
#define log_debug(fmt,args...)	printf(fmt ,##args)
#else
#define log_debug(fmt,args...)	do {}while(0)
#endif

/*-------------------------------------------------------------------------*/
__weak int host_os_is_linux(void) {return true;}
__weak int host_os_is_known(void) {return false;}

static struct usb_request *alloc_ep_req(struct usb_ep *ep, unsigned length)
{
	struct usb_request *req;

	req = usb_ep_alloc_request(ep, 0);
	if (!req)
		return req;

	req->length = length;
	req->buf = memalign(64, length);
	if (!req->buf) {
		usb_ep_free_request(ep, req);
		req = NULL;
	}

	return req;
}

static int rdp_rx_data(void)
{
	struct rdp_dev *dev = rdp_func->dev;
	int rx_data_len, status;
	/* int delay_us = (2000 * 1000 / 50); */ 

#if defined(CONFIG_USB_DWC3) || defined(CONFIG_USB_GADGET_DWC2_OTG)
	rx_data_len = dev->out_req->length = DWC3_USB_REG_BUF_LEN;
#else
	rx_data_len = dev->out_req->length = USB_REG_BUF_LEN;
#endif
	log_debug("dev->out_req->length:%d dev->rxdata:%d, dev->out_req->buf: 0x%x\n",
		dev->out_req->length, dev->rxdata, (u32)dev->out_req->buf);

	status = usb_ep_queue(dev->out_ep, dev->out_req, 0);
	if (status) {
		error("kill %s:  resubmit %d bytes --> %d",
			dev->out_ep->name, dev->out_req->length, status);
		usb_ep_set_halt(dev->out_ep);
		return -EAGAIN;
	}

	while (!dev->rxdata/* && (delay_us--) */) {
		udelay(50);
#ifdef CONFIG_USB_DWC3
		dwc3_uboot_handle_interrupt(0);
#else
		usb_gadget_handle_interrupts();
#endif
		if (ctrlc())
			return -1;
	}
	/*
	if (delay_us <= 0) {
		printf("waiting for Host request timeout\n");
		return -1;
	}
	*/
	dev->rxdata = 0;
	printf("usb rcv %d bytes\n", dev->out_req->actual);

	return 0;
}

void rdp_tx_data(unsigned char *data, int len)
{
	struct rdp_dev *dev = rdp_func->dev;
	unsigned char *ptr = dev->in_req->buf;
	int status;

	memcpy(ptr, data, len);

	dev->in_req->length = len;

	log_debug("%s: dev->in_req->length:%d to_cpy:%d\n", __func__,
	      dev->in_req->length, sizeof(data));

	status = usb_ep_queue(dev->in_ep, dev->in_req, 0);
	if (status) {
		error("kill %s:  resubmit %d bytes --> %d",
		      dev->in_ep->name, dev->in_req->length, status);
		usb_ep_set_halt(dev->in_ep);
	}

	/* Wait until tx interrupt received */
	while (!dev->txdata) {
#ifdef CONFIG_USB_DWC3
		dwc3_uboot_handle_interrupt(0);
#else
		usb_gadget_handle_interrupts();
#endif
	}

	dev->txdata = 0;
}

void rdp_tx_data_ptr(unsigned char *data, int len)
{
	struct rdp_dev *dev = rdp_func->dev;
	unsigned char *ptr = dev->in_req->buf;
	unsigned length = dev->in_req->length;
	int status;

	dev->in_req->length = len;
	dev->in_req->buf = data;

	log_debug("%s: buf: 0x%x, len: %d\n", __func__,
	      dev->in_req->buf, dev->in_req->length);

	status = usb_ep_queue(dev->in_ep, dev->in_req, 0);
	if (status) {
		error("kill %s:  resubmit %d bytes --> %d",
		      dev->in_ep->name, dev->in_req->length, status);
		usb_ep_set_halt(dev->in_ep);
	}

	/* Wait until tx interrupt received */
	while (!dev->txdata) {
#ifdef CONFIG_USB_DWC3
		dwc3_uboot_handle_interrupt(0);
#else
		usb_gadget_handle_interrupts();
#endif
	}

	dev->txdata = 0;
	dev->in_req->buf = ptr;
	dev->in_req->length = length;
}

static void rdp_rx_tx_complete(struct usb_ep *ep, struct usb_request *req)
{
	struct rdp_dev *dev = rdp_func->dev;
	int status = req->status;

	log_debug("%s: ep_ptr:%p, req_ptr:%p\n", __func__, ep, req);
	switch (status) {
	case 0:
		if (ep == dev->out_ep)
			dev->rxdata = 1;
		else
			dev->txdata = 1;

		break;

	/* this endpoint is normally active while we're configured */
	case -ECONNABORTED:		/* hardware forced ep reset */
	case -ECONNRESET:		/* request dequeued */
	case -ESHUTDOWN:		/* disconnect from host */
	case -EREMOTEIO:                /* short read */
	case -EOVERFLOW:
		error("ERROR:%d", status);
		break;
	}

	log_debug("%s complete --> %d, %d/%d\n", ep->name,
	      status, req->actual, req->length);
}

static struct usb_request *rdp_start_ep(struct usb_ep *ep)
{
	struct usb_request *req;

#if defined(CONFIG_USB_DWC3) || defined(CONFIG_USB_GADGET_DWC2_OTG)
	req = alloc_ep_req(ep, DWC3_USB_REG_BUF_LEN);
#else
	req = alloc_ep_req(ep, USB_REG_BUF_LEN);
#endif

	log_debug("%s: ep:%p req:%p\n", __func__, ep, req);

	if (!req)
		return NULL;

	memset(req->buf, 0, req->length);
	req->complete = rdp_rx_tx_complete;

	return req;
}

static void rdp_setup_complete(struct usb_ep *ep, struct usb_request *req)
{
	if (req->status || req->actual != req->length)
		log_debug("setup complete --> %d, %d/%d\n",
		      req->status, req->actual, req->length);
}

static int
rdp_func_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
{
	struct rdp_dev *dev = rdp_func->dev;
	struct usb_request *req = dev->req;
	struct usb_gadget *gadget = dev->gadget;
	int value = 0;

	u16 len = le16_to_cpu(ctrl->wLength);

	log_debug("Req_Type: 0x%x Req: 0x%x wValue: 0x%x wIndex: 0x%x wLen: 0x%x\n",
	      ctrl->bRequestType, ctrl->bRequest, ctrl->wValue, ctrl->wIndex,
	      ctrl->wLength);

	return value;
}

int rdp_init(int rdp_type)
{
	int timeout;
	int loops = 1;

	struct rdp_dev *dev = rdp_func->dev;
	struct usb_gadget *gadget = dev->gadget;

#ifdef CONFIG_DUMP2PC_IN_UBOOT
	printf("ramdump: CONFIG_DUMP2PC_IN_UBOOT enabled\n");
#endif

	if (!usb_vbus_is_online()) {
		udc_disconnect();
		printf("vbus offline, return and boot to linux\n");
		return 0;
	}

	/* Wait for a device enumeration and configuration settings */
	printf("waiting for enum done....\n");
	timeout = (2500 * 1000 / 50); //waiting for 3s
	while (!dev->configuration_done && (timeout--)) {
#ifdef CONFIG_USB_DWC3
		dwc3_uboot_handle_interrupt(0);
#else
		usb_gadget_handle_interrupts();
#endif
		udelay(50);
		/* 
		* do not wait for handshaking with non-linux os, as handshake is 
		* is only needed with CPE version rdp_transfer
		*/
		if (!host_os_is_linux() && host_os_is_known()) {
			udc_disconnect();
			printf("returns to rdp for non-linux os\n");
			return 0;
		}
	}
	if (timeout <= 0) {
		printf("waiting for enum done2....\n");
		while(loops--) {
			usb_gadget_disconnect(gadget);
			udelay(50);
			usb_gadget_connect(gadget);
			timeout = (5000 * 1000 / 50); //waiting for 3s
			while (!dev->configuration_done && (timeout--)) {
#ifdef CONFIG_USB_DWC3
				dwc3_uboot_handle_interrupt(0);
#else
				usb_gadget_handle_interrupts();
#endif
				udelay(50);
				/* 
				* do not wait for handshaking with non-linux os, as handshake is 
				* is only needed with CPE version rdp_transfer
				*/
				if (!host_os_is_linux() && host_os_is_known()) {
					udc_disconnect();
					printf("returns with non-linux os\n");
					return 0;
				}
			}

			if (timeout > 0)
				break;

			/* force to quit the while loop if linux is detected */
			if (host_os_is_linux() && timeout <= 0) {
				timeout = 1;
				break;
			}

			if (ctrlc())
				return -1;
		}
	}

	if (timeout <= 0) {
		printf("usb enum timeout\n");
		return -1;
	}
	printf("usb enum done....\n");

	if (!host_os_is_linux()) {
		udc_disconnect();
		printf("returns with non-linux os\n");
		return 0;
	}

	/* wait for HOST request */	
	if (rdp_rx_data() < 0) {
		printf("%s: rdp_rx_data failed!\n", __func__);
		return -1;
	}

	if (!strncmp((char *)dev->out_req->buf, "rdp_upload", strlen("rdp_upload"))) {
		puts("get rdp_upload\n");
		udelay(30 * 1000); /* 30 ms */
	} else {
		puts("err reply\n");
		return -1;
	}

	if (rdp_type == RDP_RAMDUMP) {
		printf("send rdp_ramdump to AP\n");
		//send text to host
		rdp_tx_data("rdp_ramdump", 11);
	} else {
		printf("send rdp_cpassert to AP\n");
		//send text to host
		rdp_tx_data("rdp_cpassert", 12);
	}

	return 0;
}

int rdp_process_data(struct rd_ram_desc *desc)
{
	unsigned char * addr = desc->addr;
	struct rdp_dev *dev = rdp_func->dev;
	u32 data_len = desc->datasize;
	u32 data_in_len = 0, idx = 0;
	u32 total_data_len = 0;
	int need_zero_pkt = 0;

	printf("send rdp_done to AP\n");
	//send text to host
	rdp_tx_data("rdp_done", 8);

	/* wait for HOST to request text */
	printf("getting rdp_text\n");
	if (rdp_rx_data() < 0) {
		printf("%s: rdp_rx_data err!\n", __func__);
		return -1;
	}
	if (!strncmp((char *)dev->out_req->buf, "rdp_text", strlen("rdp_text"))) {
		puts("get rdp_text\n");
		udelay(30 * 1000); /* 30 ms */
	} else {
		puts("Wrong reply\n");
		return -1;
	}
	printf("data addr: 0x%x %d\n", addr, desc->textsize);
	rdp_tx_data(addr, desc->textsize);
	total_data_len += desc->textsize;

	/* wait for HOST to request data */
	printf("Getting rdp_data\n");
	if (rdp_rx_data() < 0) {
		printf("%s: rdp_rx_data err!\n", __func__);
		return -1;
	}
	if (!strncmp((char *)dev->out_req->buf, "rdp_data", strlen("rdp_data"))) {
		puts("get rdp_data\n");
		udelay(30 * 1000); /* 30 ms */
	} else {
		puts("Wrong reply\n");
		return -1;
	}

	//rdp_tx_data(&data_len, sizeof(data_len));
	need_zero_pkt = ((data_len % DATA_BLOCK_LEN) == 0);
	addr = addr + desc->textsize;
	printf("data addr: 0x%x\n", addr);
	while(data_len) {
		if (data_len >= DATA_BLOCK_LEN) {
			rdp_tx_data_ptr(addr + data_in_len, DATA_BLOCK_LEN);
			data_in_len += DATA_BLOCK_LEN;
			data_len -= DATA_BLOCK_LEN;
			total_data_len += DATA_BLOCK_LEN;
		} else {
			rdp_tx_data_ptr(addr + data_in_len, data_len);
			data_in_len += data_len;
			total_data_len += data_len;
			data_len = 0;
		}
		idx++;
		if ((idx % 3000) == 0)
			printf("sending 0x%x bytes now\n", idx * DATA_BLOCK_LEN);
	}

	if (need_zero_pkt) {
		printf("zero pkt\n");
		rdp_tx_data_ptr(addr, 0);
	}

	printf("!! tx data done - total: 0x%x bytes, left: 0x%x bytes\n\n",
				total_data_len, data_len);
	udc_disconnect();
	mdelay(3000);
	return 0;
}

int rdp_process_cpassert_data(u8 *data_addr, int size)
{
	unsigned char * addr = data_addr;
	u32 data_len = size;
	struct rdp_dev *dev = rdp_func->dev;
	u32 data_in_len = 0, idx = 0, total_data_len = 0;
	int need_zero_pkt = 0;

	printf("send rdp_cp_done to AP\n");
	//send text to host
	rdp_tx_data("rdp_cp_done", 11);

	need_zero_pkt = ((size % DATA_BLOCK_LEN) == 0);
	/* wait for HOST to request text */
	printf("Getting rdp_cpassert\n");
	if (rdp_rx_data() < 0) {
		printf("%s: rdp_rx_data err!\n", __func__);
		return -1;
	}
	if (!strncmp((char *)dev->out_req->buf, "rdp_cpassert", strlen("rdp_cpassert"))) {
		puts("get rdp_cpassert\n");
		udelay(30 * 1000); /* 30 ms */
	} else {
		puts("Wrong reply\n");
		return -1;
	}

	printf("cpassert data addr: 0x%x, size: 0x%x\n", addr, data_len);
	while(data_len) {
		if (data_len >= DATA_BLOCK_LEN) {
			rdp_tx_data_ptr(addr + data_in_len, DATA_BLOCK_LEN);
			data_in_len += DATA_BLOCK_LEN;
			data_len -= DATA_BLOCK_LEN;
			total_data_len += DATA_BLOCK_LEN;
		} else {
			rdp_tx_data_ptr(addr + data_in_len, data_len);
			data_in_len += data_len;
			total_data_len += data_len;
			data_len = 0;
		}
		idx++;
		if ((idx % 3000) == 0)
			printf("sending 0x%x bytes now\n", idx * DATA_BLOCK_LEN);
	}
	if (need_zero_pkt) {
		printf("zero pkt\n");
		rdp_tx_data_ptr(addr, 0);
	}

	printf("!! CPASSERT tx data done - total: 0x%x bytes, left: 0x%x bytes\n\n",
				total_data_len, data_len);
	udelay(10000);
	return 0;
}

int rdp_give_cpassert_extra_files(u8 *data_addr, int size)
{
	unsigned char * addr = data_addr;
	u32 data_len = size;
	struct rdp_dev *dev = rdp_func->dev;
	u32 data_in_len = 0, idx = 0, total_data_len = 0;
	int need_zero_pkt = 0;

	need_zero_pkt = ((size % DATA_BLOCK_LEN) == 0);
	/* wait for HOST to request text */
	printf("Getting cpassert ext dump req\n");
	if (rdp_rx_data() < 0) {
		printf("%s: rdp_rx_data err!\n", __func__);
		return -1;
	}
	if (!strncmp((char *)dev->out_req->buf, "cpassert_extra_dump", strlen("cpassert_extra_dump"))) {
		puts("get cpassert_extra_dump\n");
		udelay(30 * 1000); /* 30 ms */
	} else {
		puts("Wrong request, check rdp_transfer tool version\n");
		return -1;
	}

	printf("cpassert extra data hdr: 0x%x, size: 0x%x\n", addr, data_len);
	while(data_len) {
		if (data_len >= DATA_BLOCK_LEN) {
			rdp_tx_data_ptr(addr + data_in_len, DATA_BLOCK_LEN);
			data_in_len += DATA_BLOCK_LEN;
			data_len -= DATA_BLOCK_LEN;
			total_data_len += DATA_BLOCK_LEN;
		} else {
			rdp_tx_data_ptr(addr + data_in_len, data_len);
			data_in_len += data_len;
			total_data_len += data_len;
			data_len = 0;
		}
		idx++;
		if ((idx % 3000) == 0)
			printf("sending 0x%x bytes now\n", idx * DATA_BLOCK_LEN);
	}
	if (need_zero_pkt) {
		printf("zero pkt\n");
		rdp_tx_data_ptr(addr, 0);
	}

	printf("!! cpassert extra data done - total: 0x%x bytes, left: 0x%x bytes\n\n",
				total_data_len, data_len);
	udelay(10000);
	return 0;
}

static int rdp_func_bind(struct usb_configuration *c, struct usb_function *f)
{
	struct usb_gadget *gadget = c->cdev->gadget;
	struct f_rdp *f_rdp = func_to_rdp(f);
	struct rdp_dev *dev;
	struct usb_ep *ep;
	int status;

	rdp_func = f_rdp;
	//dev = memalign(64, sizeof(*dev));
	dev = malloc(sizeof(*dev));
	if (!dev)
		return -ENOMEM;

	memset(dev, 0, sizeof(*dev));
	dev->gadget = gadget;
	f_rdp->dev = dev;

	log_debug("%s: usb_configuration: 0x%p usb_function: 0x%p\n",
	      __func__, c, f);
	log_debug("f_rdp: 0x%p dev: 0x%p\n", f_rdp, dev);

	dev->req = usb_ep_alloc_request(gadget->ep0, 0);
	if (!dev->req) {
		status = -ENOMEM;
		goto fail;
	}

#if defined(CONFIG_USB_DWC3) || defined(CONFIG_USB_GADGET_DWC2_OTG)
	dev->req->buf = memalign(64, DWC3_USB_REG_BUF_LEN);
#else
	dev->req->buf = memalign(64, USB_REG_BUF_LEN);
#endif

	if (!dev->req->buf) {
		status = -ENOMEM;
		goto fail;
	}

	dev->req->complete = rdp_setup_complete;

	status = usb_interface_id(c, f);

	if (status < 0)
		goto fail;

	rdp_intf_data.bInterfaceNumber = status;

	usb_ep_autoconfig_reset(gadget);
	/* allocate instance-specific endpoints */
	ep = usb_ep_autoconfig(gadget, &fs_in_desc);
	if (!ep) {
		status = -ENODEV;
		goto fail;
	}

	if (gadget_is_dualspeed(gadget)) {
		log_debug("%s: gadget is dualspeed, fs_in_desc.bEndpointAddress: 0x%x\n",
		__func__, (u32)fs_in_desc.bEndpointAddress);
		hs_in_desc.bEndpointAddress =
				fs_in_desc.bEndpointAddress;
	}

	dev->in_ep = ep;
	ep->driver_data = ep;	/* claim */

	ep = usb_ep_autoconfig(gadget, &fs_out_desc);
	if (!ep) {
		status = -ENODEV;
		goto fail;
	}

	if (gadget_is_dualspeed(gadget))
		hs_out_desc.bEndpointAddress =
				fs_out_desc.bEndpointAddress;

	dev->out_ep = ep;
	ep->driver_data = ep;	/* claim */

	if (gadget_is_dualspeed(gadget)) {
		f->hs_descriptors = (struct usb_descriptor_header **)
			&hs_rdp_data_function;

		if (!f->hs_descriptors)
			goto fail;

		f->descriptors = (struct usb_descriptor_header **)
			&fs_rdp_data_function;

		if (!f->descriptors)
			goto fail;
	}

	log_debug("%s: in_ep:%p in_req:%p\n", __func__,
	      dev->in_ep, dev->in_req);
	log_debug("%s: out_ep:%p out_req:%p\n", __func__,
	      dev->out_ep, dev->out_req);
	return 0;

 fail:
 	log_debug("%s: failed\n", __func__);
	free(dev);
	return status;
}

static void free_ep_req(struct usb_ep *ep, struct usb_request *req)
{
	free(req->buf);
	usb_ep_free_request(ep, req);
}

static void rdp_unbind(struct usb_configuration *c, struct usb_function *f)
{
	struct f_rdp *f_rdp = func_to_rdp(f);
	struct rdp_dev *dev = f_rdp->dev;

	free(dev);
	memset(rdp_func, 0, sizeof(*rdp_func));
	rdp_func = NULL;
}

static void rdp_func_disable(struct usb_function *f)
{
	struct f_rdp *f_rdp = func_to_rdp(f);
	struct rdp_dev *dev = f_rdp->dev;

	log_debug("%s:\n", __func__);

	/* Avoid freeing memory when ep is still claimed */
	if (dev->in_ep->driver_data) {
		free_ep_req(dev->in_ep, dev->in_req);
		usb_ep_disable(dev->in_ep);
		dev->in_ep->driver_data = NULL;
	}

	if (dev->out_ep->driver_data) {
		dev->out_req->buf = NULL;
		usb_ep_free_request(dev->out_ep, dev->out_req);
		usb_ep_disable(dev->out_ep);
		dev->out_ep->driver_data = NULL;
	}
}

static int rdp_eps_setup(struct usb_function *f)
{
	struct usb_composite_dev *cdev = f->config->cdev;
	struct usb_gadget *gadget = cdev->gadget;
	struct rdp_dev *dev = rdp_func->dev;
	struct usb_endpoint_descriptor *d;
	struct usb_request *req;
	struct usb_ep *ep;
	int result;

	ep = dev->in_ep;
	d = ep_desc(gadget, &hs_in_desc, &fs_in_desc);
	log_debug("in bEndpointAddress: 0x%x\n", d->bEndpointAddress);

	result = usb_ep_enable(ep, d);
	if (result)
		goto exit;

	ep->driver_data = cdev; /* claim */
	req = rdp_start_ep(ep);
	if (!req) {
		usb_ep_disable(ep);
		result = -EIO;
		goto exit;
	}
	dev->in_req = req;

	ep = dev->out_ep;
	d = ep_desc(gadget, &hs_out_desc, &fs_out_desc);
	log_debug("bEndpointAddress: 0x%x\n", d->bEndpointAddress);

	result = usb_ep_enable(ep, d);
	if (result)
		goto exit;

	ep->driver_data = cdev; /* claim */
	req = rdp_start_ep(ep);
	if (!req) {
		usb_ep_disable(ep);
		result = -EIO;
		goto exit;
	}
	dev->out_req = req;

 exit:
	return result;
}

static int rdp_func_set_alt(struct usb_function *f,
			     unsigned intf, unsigned alt)
{
	struct rdp_dev *dev = rdp_func->dev;
	int result;

	log_debug("%s: func: %s intf: %d alt: %d\n",
	      __func__, f->name, intf, alt);

	switch (intf) {
	case 0:
		log_debug("%s: intf == 0\n", __func__);
		result = rdp_eps_setup(f);
		if (result)
			error("%s: EPs setup failed!", __func__);
		dev->configuration_done = 1;
		break;
	default: 
		printf("%s: error interface!", __func__);
	}

	return 0;
}

static int rdp_func_init(struct usb_configuration *c)
{
	struct f_rdp *f_rdp;
	int status;

	log_debug("%s: cdev: 0x%p\n", __func__, c->cdev);

	//f_rdp = memalign(64, sizeof(*f_rdp));
	f_rdp = malloc(sizeof(*f_rdp));
	if (!f_rdp) {
		printf("%s: alloc f_rdp failed\n", __func__);
		return -ENOMEM;
	}
	memset(f_rdp, 0, sizeof(*f_rdp));

	f_rdp->usb_function.name = "f_rdp";
	f_rdp->usb_function.bind = rdp_func_bind;
	f_rdp->usb_function.unbind = rdp_unbind;
	f_rdp->usb_function.setup = rdp_func_setup;
	f_rdp->usb_function.set_alt = rdp_func_set_alt;
	f_rdp->usb_function.disable = rdp_func_disable;

	status = usb_add_function(c, &f_rdp->usb_function);
	if (status)
		free(f_rdp);

	return status;
}

int rdp_add(struct usb_configuration *c)
{
	log_debug("%s:\n", __func__);
	return rdp_func_init(c);
}