#include "usbdevice.h"
#include <string.h>
#include "logs_out.h"
#include <unistd.h>
#include "usbstring.h"
#include "default_cfg.h"
#include <stdio.h>
#include <iostream>
#include <errno.h>
#include <time.h>
#include "threadex.h"
#include "ThreadPool.h"

#ifndef WIN32
#include <stdlib.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#endif

#define CONFIG_VALUE 1

#ifdef AIO_BULK_TRANSFER
#define IOCB_FLAG_RESFD         (1 << 0)
#include <sys/eventfd.h>
#define BUFS_MAX	32
#endif

static int get_system_output(const char *cmd)
{
    FILE *fp=NULL;  
    fp = popen(cmd, "r");   
    if (fp)
        pclose(fp); 

    return 0;
}


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

typedef struct camtp_device_status_ {
	uint16_t wLength;
	uint16_t wCode;
}camtp_device_status;

const int UsbDevice::cacheSize = 3*1024*1024;


int camtp_load_config_file(camtp_ctx * context, const char * conffile)
{
	JsonConfig js;
    ScannerScanInfo info = js.GetScanInfo();
	int err = 0;
	FILE * f;
	char line[MAX_CFG_STRING_SIZE];

	memset((void*)&context->usb_cfg,0x00, sizeof(camtp_usb_cfg));

	// Set default config
	strncpy(context->usb_cfg.usb_device_path,         USB_DEV,                MAX_CFG_STRING_SIZE);
	strncpy(context->usb_cfg.usb_endpoint_in,         USB_EPIN,               MAX_CFG_STRING_SIZE);
	strncpy(context->usb_cfg.usb_endpoint_out,        USB_EPOUT,              MAX_CFG_STRING_SIZE);
	strncpy(context->usb_cfg.usb_endpoint_intin,      USB_EPINTIN,            MAX_CFG_STRING_SIZE);
	strncpy(context->usb_cfg.usb_string_manufacturer, MANUFACTURER,           MAX_CFG_STRING_SIZE);
	strncpy(context->usb_cfg.usb_string_product,      PRODUCT,                MAX_CFG_STRING_SIZE);
	strncpy(context->usb_cfg.usb_string_serial,       SERIALNUMBER,           MAX_CFG_STRING_SIZE);
	strncpy(context->usb_cfg.usb_string_version,      "Rev A",                MAX_CFG_STRING_SIZE);
	strncpy(context->usb_cfg.usb_string_interface,    "Ht UsbCam",            MAX_CFG_STRING_SIZE);

	context->usb_cfg.usb_vendor_id       = info.VID;
	context->usb_cfg.usb_product_id      = info.PID;
	context->usb_cfg.usb_class           = USB_DEV_CLASS;
	context->usb_cfg.usb_subclass        = USB_DEV_SUBCLASS;
	context->usb_cfg.usb_protocol        = USB_DEV_PROTOCOL;
	context->usb_cfg.usb_dev_version     = USB_DEV_VERSION;
	context->usb_cfg.usb_max_packet_size = MAX_PACKET_SIZE;
#ifndef AIO_BULK_TRANSFER
	context->usb_cfg.usb_functionfs_mode = USB_FFS_MODE;
#else
	context->usb_cfg.usb_functionfs_mode = USB_FFS_AIO_MODE;
#endif

	context->usb_cfg.wait_connection = 0;

	context->usb_cfg.loop_on_disconnect = 1;//usb live forever 

	context->usb_cfg.show_hidden_files = 1;

	context->usb_cfg.val_umask = -1;

	PRINT_MSG("USB Device path : %s",context->usb_cfg.usb_device_path);
	PRINT_MSG("USB In End point path : %s",context->usb_cfg.usb_endpoint_in);
	PRINT_MSG("USB Out End point path : %s",context->usb_cfg.usb_endpoint_out);
	PRINT_MSG("USB Event End point path : %s",context->usb_cfg.usb_endpoint_intin);
	PRINT_MSG("USB Max packet size : 0x%X bytes",context->usb_cfg.usb_max_packet_size);

	PRINT_MSG("Manufacturer string : %s",context->usb_cfg.usb_string_manufacturer);
	PRINT_MSG("Product string : %s",context->usb_cfg.usb_string_product);
	PRINT_MSG("Serial string : %s",context->usb_cfg.usb_string_serial);
	PRINT_MSG("Firmware Version string : %s", context->usb_cfg.usb_string_version);
	PRINT_MSG("Interface string : %s",context->usb_cfg.usb_string_interface);

	PRINT_MSG("USB Vendor ID : 0x%.4X",context->usb_cfg.usb_vendor_id);
	PRINT_MSG("USB Product ID : 0x%.4X",context->usb_cfg.usb_product_id);
	PRINT_MSG("USB class ID : 0x%.2X",context->usb_cfg.usb_class);
	PRINT_MSG("USB subclass ID : 0x%.2X",context->usb_cfg.usb_subclass);
	PRINT_MSG("USB Protocol ID : 0x%.2X",context->usb_cfg.usb_protocol);
	PRINT_MSG("USB Device version : 0x%.4X",context->usb_cfg.usb_dev_version);

	if(context->usb_cfg.usb_functionfs_mode == USB_FFS_MODE)
	{
		PRINT_MSG("USB FunctionFS Mode");
	}
	else if(context->usb_cfg.usb_functionfs_mode == USB_FFS_AIO_MODE)
	{
		PRINT_MSG("USB FunctionFS AIO Mode");
	}
	else
	{
		PRINT_MSG("USB GadgetFS Mode");
	}

	PRINT_MSG("Wait for connection : %i",context->usb_cfg.wait_connection);
	PRINT_MSG("Loop on disconnect : %i",context->usb_cfg.loop_on_disconnect);
	PRINT_MSG("Show hidden files : %i",context->usb_cfg.show_hidden_files);
	if(context->usb_cfg.val_umask >= 0)
	{
		PRINT_MSG("File creation umask : %03o",context->usb_cfg.val_umask);
	}
	else
	{
		PRINT_MSG("File creation umask : System default umask");
	}

	return err;
}

void UsbDevice::fill_if_descriptor(camtp_ctx * ctx, usb_gadget * usbctx, struct usb_interface_descriptor * desc)
{
	memset(desc,0,sizeof(struct usb_interface_descriptor));

	desc->bLength = sizeof(struct usb_interface_descriptor);
	desc->bDescriptorType = USB_DT_INTERFACE; //!< nick
	desc->bInterfaceNumber = 0;
	desc->iInterface = 1;
	desc->bAlternateSetting = 0;
	desc->bNumEndpoints = 3;

	desc->bInterfaceClass =    ctx->usb_cfg.usb_class;
	desc->bInterfaceSubClass = ctx->usb_cfg.usb_subclass;
	desc->bInterfaceProtocol = ctx->usb_cfg.usb_protocol;
	PRINT_MSG("usb:%d,%d,%d\n", desc->bInterfaceClass, desc->bInterfaceSubClass, desc->bInterfaceProtocol);
	if( ctx->usb_cfg.usb_functionfs_mode )
	{
		desc->iInterface = 1;
	}
	else
	{
		desc->iInterface = STRINGID_INTERFACE;
	}

	PRINT_DEBUG("fill_if_descriptor:");
	PRINT_DEBUG_BUF(desc, sizeof(struct usb_interface_descriptor));

	return;
}

void UsbDevice::fill_ep_descriptor(camtp_ctx * ctx, usb_gadget * usbctx,struct usb_endpoint_descriptor_no_audio * desc,int index,unsigned int flags)
{
	memset(desc,0,sizeof(struct usb_endpoint_descriptor_no_audio));

	desc->bLength = USB_DT_ENDPOINT_SIZE;
	desc->bDescriptorType = USB_DT_ENDPOINT;

	if(flags & EP_OUT_DIR)
		desc->bEndpointAddress = USB_DIR_OUT | (index);
	else
		desc->bEndpointAddress = USB_DIR_IN  | (index);

	if(flags & EP_BULK_MODE)
	{
		desc->bmAttributes = USB_ENDPOINT_XFER_BULK;
		if(flags & EP_SS_MODE)
		{
			desc->wMaxPacketSize = 1024;
			//  desc->wMaxPacketSize = 256;
		}else{
			desc->wMaxPacketSize = 512;
			//  desc->wMaxPacketSize = 128;
		}
	}
	else
	{
		desc->bmAttributes =  USB_ENDPOINT_XFER_INT;
		desc->wMaxPacketSize = 64; // HS size
		desc->bInterval = 6;
	}

#if defined(CONFIG_USB_SS_SUPPORT)
	if(flags & EP_SS_MODE)
	{
		ep_cfg_descriptor * ss_descriptor;

		ss_descriptor = (ep_cfg_descriptor *)desc;

		ss_descriptor->ep_desc_comp.bLength = sizeof(struct usb_ss_ep_comp_descriptor);
		ss_descriptor->ep_desc_comp.bDescriptorType = USB_DT_SS_ENDPOINT_COMP;
//		ss_descriptor->ep_desc_comp.bMaxBurst = 15;
		if(flags & EP_BULK_MODE)
		{
			ss_descriptor->ep_desc_comp.bMaxBurst = 15;
			ss_descriptor->ep_desc_comp.wBytesPerInterval = 0x00;
		}
		else
		{
			ss_descriptor->ep_desc_comp.bMaxBurst = 0;
			ss_descriptor->ep_desc_comp.wBytesPerInterval = 0x1c;
		}
		
	}
#endif

	PRINT_DEBUG("fill_ep_descriptor:");
	PRINT_DEBUG_BUF(desc, sizeof(struct usb_endpoint_descriptor_no_audio));

	return;
}

int UsbDevice::add_usb_string(usb_gadget * usbctx, int id, const char * string)
{
	int i;

	i = 0;

	while( i < MAX_USB_STRING )
	{
		if( !usbctx->stringtab[i].id )
		{
			usbctx->stringtab[i].id = id;
			if(string)
			{
				usbctx->stringtab[i].str = (char*)malloc(strlen(string) + 1);
				if(usbctx->stringtab[i].str)
				{
					memset(usbctx->stringtab[i].str,0,strlen(string) + 1);
					strcpy(usbctx->stringtab[i].str,string);
					return i;
				}
				else
				{
					usbctx->stringtab[i].id = 0;
					return -2;
				}
			}
			else
			{
				return i;
			}
		}
		i++;
	}

	return -1;
}

void UsbDevice::fill_config_descriptor(camtp_ctx * ctx , usb_gadget * usbctx,struct usb_config_descriptor * desc,int total_size, int hs)
{
	memset(desc,0,sizeof(struct usb_config_descriptor));

	desc->bLength = sizeof(struct usb_config_descriptor);
	desc->bDescriptorType = USB_DT_CONFIG;
	desc->wTotalLength = desc->bLength + total_size;
	desc->bNumInterfaces = 1;
	desc->bConfigurationValue = CONFIG_VALUE;
	if(hs)
		desc->iConfiguration = STRINGID_CONFIG_HS;
	else
		desc->iConfiguration = STRINGID_CONFIG_LS;
	desc->bmAttributes = USB_CONFIG_ATT_ONE;
	desc->bMaxPower = 1;

	PRINT_DEBUG("fill_config_descriptor: (Total Len : %u + %d = %d)", (unsigned int) sizeof(struct usb_config_descriptor), total_size, desc->wTotalLength);
	PRINT_DEBUG_BUF(desc, sizeof(struct usb_config_descriptor));

	return;
}

void UsbDevice::fill_dev_descriptor(camtp_ctx * ctx, usb_gadget * usbctx,struct usb_device_descriptor * desc)
{
	memset(desc,0,sizeof(struct usb_device_descriptor));

	desc->bLength = USB_DT_DEVICE_SIZE;
	desc->bDescriptorType = USB_DT_DEVICE;
	desc->bDeviceClass =    ctx->usb_cfg.usb_class;
	desc->bDeviceSubClass = ctx->usb_cfg.usb_subclass;
	desc->bDeviceProtocol = ctx->usb_cfg.usb_protocol;
	desc->idVendor =        ctx->usb_cfg.usb_vendor_id;
	desc->idProduct =       ctx->usb_cfg.usb_product_id;
	desc->bcdDevice =       ctx->usb_cfg.usb_dev_version; // Version
	// Strings
	desc->iManufacturer =   STRINGID_MANUFACTURER;
	desc->iProduct =        STRINGID_PRODUCT;
	desc->iSerialNumber =   STRINGID_SERIAL;
	desc->bNumConfigurations = 1; // Only one configuration

	PRINT_DEBUG("fill_dev_descriptor:");
	PRINT_DEBUG_BUF(desc, sizeof(struct usb_device_descriptor));

	return;
}

UsbDevice::UsbDevice(std::function<bool(int, struct usb_ctrlrequest *, unsigned char *)> handler, std::function<void(bool)> call_back)
: b_connected(false)
, intPointPool(new ThreadPool(10))
, connect_call(call_back)
{
	camtp_context.reset(new camtp_ctx);
	memset(camtp_context.get(), 0, sizeof(camtp_ctx));
	ctrl_handler = handler;
	std::this_thread::sleep_for(std::chrono::milliseconds(10));
	thread_main = std::move(std::thread(&UsbDevice::usb_main, this));
	// get_system_output(R"(echo petalinux:petalinux | sudo -S sh -c "ls /sys/class/udc/ > /var/cfg/usb_gadget/g1/UDC")");
	get_system_output(R"(echo root:root | sh -c "ls /sys/class/udc/ > /var/cfg/usb_gadget/g1/UDC")");
#ifdef AIO_BULK_TRANSFER
	aio_event_thread = std::move(std::thread(&UsbDevice::aio_event_poll, this));
#endif
}

UsbDevice::~UsbDevice()
{
#ifdef AIO_BULK_TRANSFER
	pthread_cancel(aio_event_thread.native_handle());
#endif
	pthread_cancel(thread_main.native_handle());
}

int UsbDevice::read_bulk(void *data, int size ,int timeOutMs )
{
	return read(usb_ctx->ep_handles[EP_DESCRIPTOR_OUT], data, size , timeOutMs);
}

//AIO 驱动支持
//#include <aio.h>
//aio_write(usb_ctx->ep_handles[EP_DESCRIPTOR_IN], data, size);
int UsbDevice::write_bulk(void *data, int size)
{
#ifdef AIO_BULK_TRANSFER
	iocb *_iocb = new iocb;
	io_prep_pwrite(_iocb, usb_ctx->ep_handles[EP_DESCRIPTOR_IN], data, size, 0);
	_iocb->u.c.flags |= IOCB_FLAG_RESFD;
	_iocb->u.c.resfd = evfd;
	int ret = io_submit(aio_ctx, 1, &_iocb);
	aio_cb.push(_iocb);
	return ret;
#else
	return write(usb_ctx->ep_handles[EP_DESCRIPTOR_IN], data, size);
#endif
}

#ifdef AIO_BULK_TRANSFER
void UsbDevice::aio_cancel()
{
	std::lock_guard<std::mutex> lck(mx);
	int aio_cb_num = aio_cb.size();
	if(aio_cb_num != 0)
	{
		for(int index = 0; index < aio_cb_num; index++)
		{
			struct io_event e[BUFS_MAX];
			iocb *temp = aio_cb.front();
			aio_cb.pop();
			io_cancel(aio_ctx, temp, e);
			delete temp;
		}
	}
}
#endif

int UsbDevice::write_int(void *data, unsigned char size)
{
	intPointPool->enqueue([this, data , size]() 
	{
		unsigned char* cpData = (unsigned char*)malloc(size);
		memcpy(cpData , data , size);
		ThreadEx senThread(std::bind(&UsbDevice::write , this ,this->usb_ctx->ep_handles[EP_DESCRIPTOR_INT_IN], cpData, size ));
		senThread.cannel(300);

		free(cpData) ;
	});
	std::this_thread::sleep_for(std::chrono::milliseconds(3));
	return 0 ;
	// return write(this->usb_ctx->ep_handles[EP_DESCRIPTOR_INT_IN], data, size) ;
}

int UsbDevice::write(int fd, void* data, size_t size)
{
    int writed = 0;
    int writing = 0;
    do
    {      
        writing = std::max(0, std::min(cacheSize, (int)(size - writed)));
		writing =  ::write(fd, (char*)data + writed, writing); 
        if(writing > 0)
		{
            writed += writing;
		}else
		{
            std::cout<<"Usb Write Has Error:"<<strerror(errno)<<std::endl;
			return writing;
		}
    
    }while (writed != size);
    
    return writed;
}


int UsbDevice::read(int fd, void* data, size_t size,int timeOutMs)
{
	long ms; // Milliseconds
	struct timespec spec , specend;
	clock_gettime(CLOCK_REALTIME, &spec);
    int readed= 0;
    int reading = 0;
	//printf("fd :%d\r\n",fd);
    do
    {
        reading = std::max(0, std::min(cacheSize, (int)(size - readed)));
		
        reading =  ::read(fd, (char*)data + readed, reading);      
        if(reading > 0)
            readed += reading;
        else
            break;
		if(timeOutMs < 0)
			continue;
		clock_gettime(CLOCK_REALTIME, &specend);
		ms = (specend.tv_nsec - spec.tv_nsec) / 1.0e6;
		if( ms > timeOutMs)
			break;    
    }while (readed != size);
    
    return readed;
}

void UsbDevice::flush_bluk_buffer()
{
	ioctl (usb_ctx->ep_handles[EP_DESCRIPTOR_IN],FUNCTIONFS_FIFO_FLUSH );	
}

void UsbDevice::reset()
{
	pthread_cancel(thread_main.native_handle());
	// thread_main = std::move(std::thread(&UsbDevice::usb_main, this));
}

void UsbDevice::showUsbStatus()
{
	int readData = 0 ;
	int ret = 0 ;
	ret = ioctl(usb_ctx->ep_handles[EP_DESCRIPTOR_IN] , FUNCTIONFS_FIFO_STATUS , &readData);
	std::cout << " read  EP BULK IN Ret " << ret  << " data " <<  readData << std::endl ;

	// ret = ioctl(usb_ctx , FUNCTIONFS_FIFO_FLUSH);
	// std::cout << " read  USB Ret " << ret  << " data " <<  readData << std::endl ;
	// ret = ioctl(ctx->usb_device+1 , FUNCTIONFS_FIFO_STATUS , &readData);
	// std::cout << " read  EP BULK IN Ret" << ret  << " data " <<  readData << std::endl ;
}

void UsbDevice::resetUSB()
{
	int readData = 0 ;
	int ret = 0 ;
	ret = ioctl(usb_ctx->ep_handles[EP_DESCRIPTOR_IN] , FUNCTIONFS_CLEAR_HALT , &readData);
	std::cout << " read  EP BULK IN Ret " << ret  << " data " <<  readData << std::endl ;

	ret = ioctl(usb_ctx->usb_device+1 , FUNCTIONFS_CLEAR_HALT , &readData);
	std::cout << " read  EP Contorl  Ret " << ret  << " data " <<  readData << std::endl ;
}

void UsbDevice::abort_int()
{
	ioctl (usb_ctx->ep_handles[EP_DESCRIPTOR_INT_IN], GADGETFS_FIFO_FLUSH);
}

usb_gadget * UsbDevice::init_usb_camtp_gadget(camtp_ctx * ctx)
{
	usb_gadget * usbctx;
	int cfg_size;
	int ret,i;
	ffs_strings ffs_str;

	usbctx = NULL;

	usbctx = (usb_gadget *)malloc(sizeof(usb_gadget));
	if(usbctx)
	{

		memset(usbctx,0,sizeof(usb_gadget));

		usbctx->usb_device = -1;
		usbctx->thread_not_started = 1;

		i = 0;
		while( i < EP_NB_OF_DESCRIPTORS )
		{
			usbctx->ep_handles[i] = -1;
			i++;
		}

		add_usb_string(usbctx, STRINGID_MANUFACTURER, ctx->usb_cfg.usb_string_manufacturer);
		add_usb_string(usbctx, STRINGID_PRODUCT,      ctx->usb_cfg.usb_string_product);
		add_usb_string(usbctx, STRINGID_SERIAL,       ctx->usb_cfg.usb_string_serial);
		add_usb_string(usbctx, STRINGID_CONFIG_HS,    "High speed configuration");
		add_usb_string(usbctx, STRINGID_CONFIG_LS,    "Low speed configuration");
		add_usb_string(usbctx, STRINGID_INTERFACE,    ctx->usb_cfg.usb_string_interface);
		add_usb_string(usbctx, STRINGID_MAX,          NULL);

		strings.strings = usbctx->stringtab;

		usbctx->wait_connection = ctx->usb_cfg.wait_connection;

		for(i=0;i<3;i++)
		{
			usbctx->ep_config[i] = (ep_cfg*)malloc(sizeof(ep_cfg));
			if(!usbctx->ep_config[i])
				goto init_error;

			memset(usbctx->ep_config[i],0,sizeof(ep_cfg));
		}

		usbctx->ep_path[0] = ctx->usb_cfg.usb_endpoint_in;
		usbctx->ep_path[1] = ctx->usb_cfg.usb_endpoint_out;
		usbctx->ep_path[2] = ctx->usb_cfg.usb_endpoint_intin;

		usbctx->usb_device = open(ctx->usb_cfg.usb_device_path, O_RDWR|O_SYNC);

		if (usbctx->usb_device <= 0)
		{
			PRINT_ERROR("init_usb_camtp_gadget : Unable to open %s (%m)", ctx->usb_cfg.usb_device_path);
			goto init_error;
		}

		cfg_size = sizeof(struct usb_interface_descriptor) + (sizeof(struct usb_endpoint_descriptor_no_audio) * 3);

		if( ctx->usb_cfg.usb_functionfs_mode )
		{
			// FunctionFS mode

			usbctx->usb_ffs_config = (usb_ffs_cfg *)malloc(sizeof(usb_ffs_cfg));
			if(!usbctx->usb_ffs_config)
				goto init_error;

			memset(usbctx->usb_ffs_config,0,sizeof(usb_ffs_cfg));

#ifdef OLD_FUNCTIONFS_DESCRIPTORS // Kernel < v3.15
			usbctx->usb_ffs_config->magic = htole32(FUNCTIONFS_DESCRIPTORS_MAGIC);
#else
			usbctx->usb_ffs_config->magic = htole32(FUNCTIONFS_DESCRIPTORS_MAGIC_V2);
			usbctx->usb_ffs_config->flags = htole32(0);
			usbctx->usb_ffs_config->flags |= htole32(FUNCTIONFS_ALL_CTRL_RECIP);

#ifdef CONFIG_USB_FS_SUPPORT
			usbctx->usb_ffs_config->flags |= htole32(FUNCTIONFS_HAS_FS_DESC);
#endif

#ifdef CONFIG_USB_HS_SUPPORT
			usbctx->usb_ffs_config->flags |= htole32(FUNCTIONFS_HAS_HS_DESC);
#endif

#ifdef CONFIG_USB_SS_SUPPORT
			usbctx->usb_ffs_config->flags |= htole32(FUNCTIONFS_HAS_SS_DESC);
#endif

#endif
			usbctx->usb_ffs_config->length = htole32(sizeof(usb_ffs_cfg));

#ifdef CONFIG_USB_FS_SUPPORT
			usbctx->usb_ffs_config->fs_count = htole32(1 + 3);

			fill_if_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_fs.if_desc);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_fs.ep_desc_in,1, EP_BULK_MODE | EP_IN_DIR);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_fs.ep_desc_out,2, EP_BULK_MODE | EP_OUT_DIR);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_fs.ep_desc_int_in,3, EP_INT_MODE | EP_IN_DIR);
#endif

#ifdef CONFIG_USB_HS_SUPPORT
			usbctx->usb_ffs_config->hs_count = htole32(1 + 3);
			fill_if_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_hs.if_desc);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_hs.ep_desc_in,1, EP_BULK_MODE | EP_IN_DIR | EP_HS_MODE);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_hs.ep_desc_out,2, EP_BULK_MODE | EP_OUT_DIR | EP_HS_MODE);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_hs.ep_desc_int_in,3, EP_INT_MODE | EP_IN_DIR | EP_HS_MODE);
#endif

#ifdef CONFIG_USB_SS_SUPPORT
			usbctx->usb_ffs_config->ss_count = htole32(1 + (3*2));
			fill_if_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_ss.if_desc);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_ss.ep_desc_in,1, EP_BULK_MODE | EP_IN_DIR | EP_SS_MODE);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_ss.ep_desc_out,2, EP_BULK_MODE | EP_OUT_DIR | EP_SS_MODE);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_ffs_config->ep_desc_ss.ep_desc_int_in,3, EP_INT_MODE | EP_IN_DIR | EP_SS_MODE);
#endif

			PRINT_DEBUG("init_usb_camtp_gadget :");
			PRINT_DEBUG_BUF(usbctx->usb_ffs_config, sizeof(usb_ffs_cfg));

			ret = write(usbctx->usb_device, usbctx->usb_ffs_config, sizeof(usb_ffs_cfg));

			if(ret != sizeof(usb_ffs_cfg))
			{
				PRINT_ERROR("FunctionFS USB Config write error (%d != %zu)",ret,sizeof(usb_ffs_cfg));
				goto init_error;
			}

			memset( &ffs_str, 0, sizeof(ffs_strings));
			ffs_str.header.magic = htole32(FUNCTIONFS_STRINGS_MAGIC);
			ffs_str.header.length = htole32(sizeof(struct usb_functionfs_strings_head) + sizeof(uint16_t) + strlen(ctx->usb_cfg.usb_string_interface) + 1);
			ffs_str.header.str_count = htole32(1);
			ffs_str.header.lang_count = htole32(1);
			ffs_str.code = htole16(0x0409); // en-us
			strcpy(ffs_str.string_data,ctx->usb_cfg.usb_string_interface);

			PRINT_DEBUG("write string :");
			PRINT_DEBUG_BUF(&ffs_str, sizeof(ffs_strings));

			ret = write(usbctx->usb_device, &ffs_str, ffs_str.header.length);

			if( ret != ffs_str.header.length )
			{
				PRINT_ERROR("FunctionFS String Config write error (%d != %zu)",ret,(size_t)ffs_str.header.length);
				goto init_error;
			}
		}
		else
		{
			usbctx->usb_config = (usb_cfg *)malloc(sizeof(usb_cfg));
			if(!usbctx->usb_config)
				goto init_error;

			memset(usbctx->usb_config,0,sizeof(usb_cfg));

			usbctx->usb_config->head = 0x00000000;

#ifdef CONFIG_USB_FS_SUPPORT
			fill_config_descriptor(ctx, usbctx, &usbctx->usb_config->cfg_fs, cfg_size, 0);
			fill_if_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_fs.if_desc);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_fs.ep_desc_in,1, EP_BULK_MODE | EP_IN_DIR);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_fs.ep_desc_out,2, EP_BULK_MODE | EP_OUT_DIR);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_fs.ep_desc_int_in,3, EP_INT_MODE | EP_IN_DIR);
#endif

#ifdef CONFIG_USB_HS_SUPPORT
			fill_config_descriptor(ctx, usbctx, &usbctx->usb_config->cfg_hs, cfg_size, 1);
			fill_if_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_hs.if_desc);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_hs.ep_desc_in,1, EP_BULK_MODE | EP_IN_DIR | EP_HS_MODE);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_hs.ep_desc_out,2, EP_BULK_MODE | EP_OUT_DIR | EP_HS_MODE);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_hs.ep_desc_int_in,3, EP_INT_MODE | EP_IN_DIR | EP_HS_MODE);
#endif

#ifdef CONFIG_USB_SS_SUPPORT
			fill_config_descriptor(ctx, usbctx, &usbctx->usb_config->cfg_ss, cfg_size, 1);
			fill_if_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_ss.if_desc);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_ss.ep_desc_in,1, EP_BULK_MODE | EP_IN_DIR | EP_SS_MODE);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_ss.ep_desc_out,2, EP_BULK_MODE | EP_OUT_DIR | EP_SS_MODE);
			fill_ep_descriptor(ctx, usbctx, &usbctx->usb_config->ep_desc_ss.ep_desc_int_in,3, EP_INT_MODE | EP_IN_DIR | EP_SS_MODE);
#endif

			fill_dev_descriptor(ctx, usbctx,&usbctx->usb_config->dev_desc);

			PRINT_DEBUG("init_usb_camtp_gadget :");
			PRINT_DEBUG_BUF(usbctx->usb_config, sizeof(usb_cfg));

			ret = write(usbctx->usb_device, usbctx->usb_config, sizeof(usb_cfg));

			if(ret != sizeof(usb_cfg))
			{
				PRINT_ERROR("GadgetFS USB Config write error (%d != %zu)",ret,sizeof(usb_cfg));
				goto init_error;
			}
		}

		PRINT_DEBUG("init_usb_camtp_gadget : USB config done");

		return usbctx;
	}

init_error:
	PRINT_ERROR("init_usb_camtp_gadget init error !");

	deinit_usb_camtp_gadget(usbctx);

	return 0;
}

void UsbDevice::deinit_usb_camtp_gadget(usb_gadget * usbctx)
{
	int i;

	PRINT_DEBUG("entering deinit_usb_camtp_gadget");

	if( usbctx )
	{
		usbctx->stop = 1;

		i = 0;
		while( i < EP_NB_OF_DESCRIPTORS )
		{

			if( usbctx->ep_handles[i] >= 0 )
			{
				PRINT_DEBUG("Closing End Point %d...",i);
				close(usbctx->ep_handles[i] );
			}
			i++;
		}

		if (usbctx->usb_device >= 0)
		{
			PRINT_DEBUG("Closing usb device...");
			close(usbctx->usb_device);
			usbctx->usb_device = - 1;
		}

		if( !usbctx->thread_not_started )
		{
			PRINT_DEBUG("Stopping USB Thread...");
			pthread_cancel (usbctx->thread);
			pthread_join(usbctx->thread, NULL);
			usbctx->thread_not_started = 1;
		}

		if(usbctx->usb_config)
		{
			free(usbctx->usb_config);
			usbctx->usb_config = 0;
		}

		if(usbctx->usb_ffs_config)
		{
			free(usbctx->usb_ffs_config);
			usbctx->usb_ffs_config = 0;
		}

		for(i=0;i<3;i++)
		{
			if( usbctx->ep_config[i] )
				free( usbctx->ep_config[i] );
		}

		i = 0;
		while( i < MAX_USB_STRING )
		{
			if( usbctx->stringtab[i].str )
			{
				free ( usbctx->stringtab[i].str );
			}
			i++;
		}

		free( usbctx );
	}

	PRINT_DEBUG("leaving deinit_usb_camtp_gadget");
}

void UsbDevice::usb_main()
{
	int retcode = 0;
	loop_continue = 0;
	camtp_load_config_file(camtp_context.get(), "");

	loop_continue = camtp_context->usb_cfg.loop_on_disconnect;

	do
	{
		usb_ctx = init_usb_camtp_gadget(camtp_context.get());
		// get_system_output(R"(echo petalinux:petalinux | sudo -S sh -c "chmod 777 /dev/ffs-camtp -R")");
		get_system_output(R"(echo root:root | sh -c "chmod 777 /dev/ffs-camtp -R")");

		if (usb_ctx)
		{
			//camtp_set_usb_handle(camtp_context.get(), usb_ctx, camtp_context->usb_cfg.usb_max_packet_size);
			camtp_context->usb_ctx = usb_ctx;
			/* FFS AIO MODE OR BLOCK MODE --- CONTROL ENDPOINT listen is same*/ 
			if (camtp_context->usb_cfg.usb_functionfs_mode == USB_FFS_MODE || camtp_context->usb_cfg.usb_functionfs_mode == USB_FFS_AIO_MODE)
			{
				PRINT_DEBUG("CAMTP Responder : FunctionFS Mode - entering handle_ffs_ep0");
				handle_ffs_ep0(usb_ctx);
			}
			// else if(camtp_context->usb_cfg.usb_functionfs_mode == USB_FFS_AIO_MODE)
			// {
			// 	PRINT_DEBUG("CAMTP Responder : FunctionFS Mode - entering handle_ffs_aio_ep0");
			// 	handle_ffs_aio_ep0(usb_ctx);
			// }
			else
			{
				PRINT_DEBUG("CAMTP Responder : GadgetFS Mode - entering handle_ep0");
				handle_ep0(usb_ctx);
			}
			deinit_usb_camtp_gadget(usb_ctx);
		}
		else
		{
			PRINT_ERROR("USB Init failed !");
			retcode = -2;
			loop_continue = 0;
		}

		PRINT_MSG("CAMTP Responder : Disconnected");

	} while (loop_continue);
	PRINT_MSG("CAMTP Responder : USB Server shutdown!");
}


// Function FS mode handler
int UsbDevice::handle_ffs_ep0(usb_gadget * ctx)
{
	struct timeval timeout;
	int ret, nevents, i;
	fd_set read_set;
	struct usb_functionfs_event event;
	int status;

	PRINT_MSG("handle_ffs_ep0 : Entering... ctx->stop = %d \n",ctx->stop);
	timeout.tv_sec = 20;
	timeout.tv_usec = 0;

	while (!ctx->stop)
	{
		FD_ZERO(&read_set);
		FD_SET(ctx->usb_device, &read_set);

		if(timeout.tv_sec)
		{
			ret = select(ctx->usb_device+1, &read_set, NULL, NULL, &timeout);
		}
		else
		{
			PRINT_DEBUG("Select without timeout");
			ret = select(ctx->usb_device+1, &read_set, NULL, NULL, NULL);
		}

		if(ctx->wait_connection && ret == 0 )
			continue;
		PRINT_MSG("Select ret = %d ",ret);
		if( ret <= 0 )
			return ret;

		timeout.tv_sec = 0;

		ret = read(ctx->usb_device, &event, sizeof(event));
		PRINT_MSG("read(ctx->usb_device ret = %d ",ret);
		if (ret < 0)
		{
			PRINT_ERROR("handle_ffs_ep0 : Read error %d (%m)", ret);
			goto end;
		}

		nevents = ret / sizeof(event);

		PRINT_MSG("%d event(s)", nevents);

		for (i=0; i<nevents; i++)
		{
			PRINT_MSG("event.type = %d ", event.type);
			switch (event.type)
			{
			case FUNCTIONFS_ENABLE:
				PRINT_MSG("FUNCTIONFS_ENABLE  Usb Connect");
				//!< nick usb on
				b_connected = true;
				if(connect_call)
					connect_call(b_connected);

				if (ctx->ep_handles[EP_DESCRIPTOR_IN] <= 0)
				{
					status = init_eps(ctx,1);
				}
				else
					status = 0;

				break;
			case FUNCTIONFS_DISABLE:
				PRINT_MSG("FUNCTIONFS_DISABLE  Usb disconnect");

				b_connected = false;
				if(connect_call)
					connect_call(b_connected);
				//!< nick usb off
				// Set timeout for a reconnection during the enumeration...
				timeout.tv_sec = 0;
				timeout.tv_usec = 0;

				// Stop the main rx thread.
				ctx->stop = 1;
				if( !ctx->thread_not_started )
				{
					pthread_join(ctx->thread, NULL);
					ctx->thread_not_started = 1;
				}
				// But don't close the endpoints !
				ctx->stop = 0;

				break;
			case FUNCTIONFS_SETUP:
				PRINT_MSG("EP0 FFS SETUP");
				handle_setup_request(ctx, &event.u.setup);
				break;
			case FUNCTIONFS_BIND:
				PRINT_MSG("EP0 FFS BIND");
				break;
			case FUNCTIONFS_UNBIND:
				PRINT_MSG("EP0 FFS UNBIND");
				break;
			case FUNCTIONFS_SUSPEND:
				PRINT_MSG("EP0 FFS SUSPEND");
				break;
			case FUNCTIONFS_RESUME:
				PRINT_MSG("EP0 FFS RESUME");
				break;
			}
		}
	}

	ctx->stop = 1;

end:
	PRINT_DEBUG("handle_ffs_ep0 : Leaving... (ctx->stop=%d)",ctx->stop);
	return 1;
}


// GadgetFS mode handler
int UsbDevice::handle_ep0(usb_gadget * ctx)
{
	struct timeval timeout;
	int ret, nevents, i, cnt;
	fd_set read_set;
	struct usb_gadgetfs_event events[5];

	PRINT_DEBUG("handle_ep0 : Entering...");
	timeout.tv_sec = 4;
	timeout.tv_usec = 0;

	while (!ctx->stop)
	{
		FD_ZERO(&read_set);
		FD_SET(ctx->usb_device, &read_set);

		if(timeout.tv_sec)
		{
			ret = select(ctx->usb_device+1, &read_set, NULL, NULL, &timeout);
		}
		else
		{
			PRINT_DEBUG("handle_ep0 : Select without timeout");
			ret = select(ctx->usb_device+1, &read_set, NULL, NULL, NULL);
		}

		if(ctx->wait_connection && ret == 0 )
			continue;

		if( ret <= 0 )
			return ret;

		timeout.tv_sec = 0;

		ret = read(ctx->usb_device, &events, sizeof(events));

		if (ret < 0)
		{
			PRINT_ERROR("handle_ep0 : Read error %d (%m)", errno);
			goto end;
		}

		nevents = ret / sizeof(events[0]);

		PRINT_DEBUG("handle_ep0 : %d event(s)", nevents);

		for (i=0; i<nevents; i++)
		{
			switch (events[i].type)
			{
				case GADGETFS_CONNECT:
					PRINT_DEBUG("handle_ep0 : EP0 CONNECT event");
				break;

				case GADGETFS_DISCONNECT:
					PRINT_DEBUG("handle_ep0 : EP0 DISCONNECT event");

					// Set timeout for a reconnection during the enumeration...
					timeout.tv_sec = 1;
					timeout.tv_usec = 0;

					ctx->stop = 1;
					if( !ctx->thread_not_started )
					{
						pthread_cancel(ctx->thread);
						pthread_join(ctx->thread, NULL);
						ctx->thread_not_started = 1;
					}
				break;

				case GADGETFS_SETUP:
					PRINT_DEBUG("handle_ep0 : EP0 SETUP event");
					handle_setup_request(ctx, &events[i].u.setup);
				break;

				case GADGETFS_NOP:
					PRINT_DEBUG("handle_ep0 : EP0 NOP event");
				break;

				case GADGETFS_SUSPEND:
					PRINT_DEBUG("handle_ep0 : EP0 SUSPEND event");

					break;

				default:
					PRINT_DEBUG("handle_ep0 : EP0 unknown event : %d",events[i].type);
				break;
			}
		}
	}

	ctx->stop = 1;

end:
	PRINT_DEBUG("handle_ep0 : Leaving (ctx->stop=%d)...",ctx->stop);

	return 1;
}

void UsbDevice::handle_setup_request(usb_gadget * ctx, struct usb_ctrlrequest* setup)
{
	int status,cnt;
	uint8_t buffer[512];
	camtp_device_status dstatus;

	PRINT_DEBUG("Setup requestType 0x%.2X", setup->bRequestType);
	PRINT_DEBUG("Setup request 0x%.2X", setup->bRequest);

	if(ctrl_handler && ctrl_handler(ctx->usb_device, setup, buffer))
		return;

	switch (setup->bRequest)
	{
		case USB_REQ_GET_DESCRIPTOR:
			if (setup->bRequestType != USB_DIR_IN)
				goto stall;

			switch (setup->wValue >> 8)
			{
				case USB_DT_STRING:
					PRINT_DEBUG("Get string id #%d (max length %d)", setup->wValue & 0xff,
						setup->wLength);
					status = usb_gadget_get_string (&strings, setup->wValue & 0xff, buffer);
					// Error
					if (status < 0)
					{
						PRINT_ERROR("handle_setup_request : String id #%d (max length %d) not found !",setup->wValue & 0xff, setup->wLength);
						break;
					}
					else
					{
						PRINT_DEBUG("Found %d bytes", status);
						PRINT_DEBUG_BUF(buffer, status);
					}

					if ( write (ctx->usb_device, buffer, status) < 0 )
					{
						PRINT_ERROR("handle_setup_request - USB_REQ_GET_DESCRIPTOR : usb device write error !");
						break;
					}

					return;
				break;
				default:
					PRINT_DEBUG("Cannot return descriptor %d", (setup->wValue >> 8));
				break;
			}
		break;

		case USB_REQ_SET_CONFIGURATION:
			if (setup->bRequestType != USB_DIR_OUT)
			{
				PRINT_DEBUG("Bad dir");
				goto stall;
			}

			switch (setup->wValue)
			{
				case CONFIG_VALUE:
					PRINT_DEBUG("Set config value");

					if (ctx->ep_handles[EP_DESCRIPTOR_IN] <= 0)
					{
						status = init_eps(ctx,0);
					}
					else
						status = 0;

					break;
				case 0:
					PRINT_DEBUG("Disable threads");
					ctx->stop = 1;
					break;

				default:
					PRINT_DEBUG("Unhandled configuration value %d", setup->wValue);
					break;
			}

			// Just ACK
			status = read (ctx->usb_device, &status, 0);
			return;
		break;

		case USB_REQ_GET_INTERFACE:
			PRINT_DEBUG("GET_INTERFACE");
			buffer[0] = 0;

			if ( write (ctx->usb_device, buffer, 1) < 0 )
			{
				PRINT_ERROR("handle_setup_request - USB_REQ_GET_INTERFACE : usb device write error !");
				break;
			}

			return;
		break;

		case USB_REQ_SET_INTERFACE:
			PRINT_DEBUG("SET_INTERFACE");
			ioctl (ctx->ep_handles[EP_DESCRIPTOR_IN], GADGETFS_CLEAR_HALT);
			ioctl (ctx->ep_handles[EP_DESCRIPTOR_OUT], GADGETFS_CLEAR_HALT);
			ioctl (ctx->ep_handles[EP_DESCRIPTOR_INT_IN], GADGETFS_CLEAR_HALT);
			// ACK
			status = read (ctx->usb_device, &status, 0);
			return;
		break;

	}

stall:
	PRINT_DEBUG("Stalled");
	// Error
	if (setup->bRequestType & USB_DIR_IN)
	{
		if ( read (ctx->usb_device, &status, 0) < 0 )
		{
			PRINT_DEBUG("handle_setup_request - stall : usb device read error !");
		}
	}
	else
	{
		if ( write (ctx->usb_device, &status, 0) < 0 )
		{
			PRINT_DEBUG("handle_setup_request - stall : usb device write error !");
		}
	}
}

int UsbDevice::init_ep(usb_gadget * ctx,int index,int ffs_mode)
{
	int fd,ret;
	void * descriptor_ptr;
	int descriptor_size;

	PRINT_MSG("Init end point %s (%d)",ctx->ep_path[index],index);
	fd = open(ctx->ep_path[index], O_RDWR);
	if ( fd <= 0 )
	{
		PRINT_MSG("init_ep : Endpoint %s (%d) init failed ! : Can't open the endpoint ! (error %d - %m)",ctx->ep_path[index],index,fd);
		goto init_ep_error;
	}

	ctx->ep_handles[index] = fd;

	ctx->ep_config[index]->head = 1;

	descriptor_size = 0;

	if( ctx->usb_ffs_config )
	{
#if defined(CONFIG_USB_SS_SUPPORT)
		descriptor_ptr = (void *)&ctx->usb_ffs_config->ep_desc_ss;
		descriptor_size = sizeof(ep_cfg_descriptor);
#elif defined(CONFIG_USB_HS_SUPPORT)
		descriptor_ptr = (void *)&ctx->usb_ffs_config->ep_desc_hs;
		descriptor_size = sizeof(struct usb_endpoint_descriptor_no_audio);
#elif defined(CONFIG_USB_FS_SUPPORT)
		descriptor_ptr = (void *)&ctx->usb_ffs_config->ep_desc_fs;
		descriptor_size = sizeof(struct usb_endpoint_descriptor_no_audio);
#else

#error Configuration Error ! At least one USB mode support must be enabled ! (CONFIG_USB_FS_SUPPORT/CONFIG_USB_HS_SUPPORT/CONFIG_USB_SS_SUPPORT)

#endif
	}
	else
	{
#if defined(CONFIG_USB_SS_SUPPORT)
		descriptor_ptr = (void *)&ctx->usb_config->ep_desc_ss;
		descriptor_size = sizeof(ep_cfg_descriptor);
#elif defined(CONFIG_USB_HS_SUPPORT)
		descriptor_ptr = (void *)&ctx->usb_config->ep_desc_hs;
		descriptor_size = sizeof(struct usb_endpoint_descriptor_no_audio);
#elif defined(CONFIG_USB_FS_SUPPORT)
		descriptor_ptr = (void *)&ctx->usb_config->ep_desc_fs;
		descriptor_size = sizeof(struct usb_endpoint_descriptor_no_audio);
#else

#error Configuration Error ! At least one USB mode support must be enabled ! (CONFIG_USB_FS_SUPPORT/CONFIG_USB_HS_SUPPORT/CONFIG_USB_SS_SUPPORT)

#endif
	}

#if defined(CONFIG_USB_SS_SUPPORT)
	switch(index)
	{
		case EP_DESCRIPTOR_IN:
			memcpy(&ctx->ep_config[index]->ep_desc[0], &((SSEndPointsDesc*)descriptor_ptr)->ep_desc_in,descriptor_size);
			memcpy(&ctx->ep_config[index]->ep_desc[1], &((SSEndPointsDesc*)descriptor_ptr)->ep_desc_in,descriptor_size);
		break;
		case EP_DESCRIPTOR_OUT:
			memcpy(&ctx->ep_config[index]->ep_desc[0], &((SSEndPointsDesc*)descriptor_ptr)->ep_desc_out,descriptor_size);
			memcpy(&ctx->ep_config[index]->ep_desc[1], &((SSEndPointsDesc*)descriptor_ptr)->ep_desc_out,descriptor_size);
		break;
		case EP_DESCRIPTOR_INT_IN:
			memcpy(&ctx->ep_config[index]->ep_desc[0], &((SSEndPointsDesc*)descriptor_ptr)->ep_desc_int_in,descriptor_size);
			memcpy(&ctx->ep_config[index]->ep_desc[1], &((SSEndPointsDesc*)descriptor_ptr)->ep_desc_int_in,descriptor_size);
		break;
	}
#else
	switch(index)
	{
		case EP_DESCRIPTOR_IN:
			memcpy(&ctx->ep_config[index]->ep_desc[0], &((EndPointsDesc*)descriptor_ptr)->ep_desc_in,descriptor_size);
			memcpy(&ctx->ep_config[index]->ep_desc[1], &((EndPointsDesc*)descriptor_ptr)->ep_desc_in,descriptor_size);
		break;
		case EP_DESCRIPTOR_OUT:
			memcpy(&ctx->ep_config[index]->ep_desc[0], &((EndPointsDesc*)descriptor_ptr)->ep_desc_out,descriptor_size);
			memcpy(&ctx->ep_config[index]->ep_desc[1], &((EndPointsDesc*)descriptor_ptr)->ep_desc_out,descriptor_size);
		break;
		case EP_DESCRIPTOR_INT_IN:
			memcpy(&ctx->ep_config[index]->ep_desc[0], &((EndPointsDesc*)descriptor_ptr)->ep_desc_int_in,descriptor_size);
			memcpy(&ctx->ep_config[index]->ep_desc[1], &((EndPointsDesc*)descriptor_ptr)->ep_desc_int_in,descriptor_size);
		break;
	}
#endif

	PRINT_DEBUG("init_ep (%d):",index);
	PRINT_DEBUG_BUF(ctx->ep_config[index], sizeof(ep_cfg));

	if(!ffs_mode)
	{
		ret = write(fd, ctx->ep_config[index], sizeof(ep_cfg));

		if (ret != sizeof(ep_cfg))
		{
			PRINT_ERROR("init_ep : Endpoint %s (%d) init failed ! : Write Error %d - %m",ctx->ep_path[index], index, ret);
			goto init_ep_error;
		}
	}
	else
	{
		PRINT_DEBUG("init_ep (%d): FunctionFS Mode - Don't write the endpoint descriptor.",index);
	}

	return fd;

init_ep_error:
	return 0;
}

int UsbDevice::init_eps(usb_gadget * ctx, int ffs_mode)
{
	if( !init_ep(ctx, EP_DESCRIPTOR_IN, ffs_mode) )
		goto init_eps_error;

	if( !init_ep(ctx, EP_DESCRIPTOR_OUT, ffs_mode) )
		goto init_eps_error;

	if( !init_ep(ctx, EP_DESCRIPTOR_INT_IN, ffs_mode) )
		goto init_eps_error;

	return 0;

init_eps_error:
	return 1;
}

#ifdef AIO_BULK_TRANSFER
int UsbDevice::aio_event_poll()
{
	int ret;
	fd_set rfds;
	int m_count = 0;
	memset(&aio_ctx, 0, sizeof(aio_ctx));
	if(io_setup(BUFS_MAX, &aio_ctx))
	{
		PRINT_DEBUG("handle_ffs_aio_ep0 : unale to setup aio");
		return 1;
	}

	evfd = eventfd(0, EFD_NONBLOCK | EFD_CLOEXEC);
	if (evfd < 0) 
	{
		PRINT_DEBUG("unable to open eventfd");
		return 1;
	}

	while(!usb_ctx){
		std::this_thread::sleep_for(std::chrono::milliseconds(1)); //wait for usb init
	}

	while(!usb_ctx->stop)
	{
		FD_ZERO(&rfds);
		FD_SET(evfd, &rfds);
		ret = select(evfd + 1, &rfds, NULL, NULL, NULL);
		if (ret < 0) 
		{
			if (errno == EINTR)
				continue;
			PRINT_DEBUG("select");
			break;
		}
		if (!FD_ISSET(evfd, &rfds))
			continue;
		uint64_t ev_cnt;
		ret = read(evfd, &ev_cnt, sizeof(ev_cnt));
		if (ret < 0) {
			PRINT_DEBUG("unable to read eventfd");
			break;
		}
		struct io_event e[BUFS_MAX];
		/* we read aio events */
		ret = io_getevents(aio_ctx, 1, BUFS_MAX, e, NULL);

		// std::cout << "io_getevent_ret: " << ret << std::endl;
		// std::cout << "aio_queue: " << aio_cb.size() << std::endl;
		{
			std::lock_guard<std::mutex> lck(mx);
			if(ret > 0 && aio_cb.size() != 0)
			{
				iocb *temp = aio_cb.front();
				aio_cb.pop();
				delete temp;
				// std::cout << "io_getevent : " << ++m_count << std::endl;
			}
		}
	}
	close(evfd);
	io_destroy(aio_ctx);
	return 0;
}
#endif