1361 lines
38 KiB
C++
Executable File
1361 lines
38 KiB
C++
Executable File
#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
|
|
|