|
#include "usbctrl.h"
|
|
#include "DspParserBase.h"
|
|
#include "ReportError.h"
|
|
|
|
#include <errno.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <memory.h>
|
|
#include <pthread.h>
|
|
#include <signal.h>
|
|
#include <stdlib.h>
|
|
|
|
#include <sys/types.h>
|
|
#include <sys/stat.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/poll.h>
|
|
#include <stdint.h>
|
|
|
|
#include <sys/syscall.h>
|
|
#include <linux/aio_abi.h>
|
|
|
|
using namespace MityDSP;
|
|
|
|
extern "C" {
|
|
|
|
#include <asm/byteorder.h>
|
|
|
|
#define le16_to_cpu __le16_to_cpu
|
|
|
|
#include <linux/types.h>
|
|
#include <linux/usb/gadgetfs.h>
|
|
#include <linux/usb/ch9.h>
|
|
|
|
#include "usbstring.h"
|
|
|
|
#define DRIVER_VENDOR_NUM 0x1EF8 /* Critical Link */
|
|
#define DRIVER_PRODUCT_NUM 0x1005 /* MityCAM */
|
|
#define STRINGID_MFGR 1
|
|
#define STRINGID_PRODUCT 2
|
|
#define STRINGID_SERIAL 3
|
|
#define STRINGID_CONFIG 4
|
|
#define STRINGID_INTERFACE 5
|
|
|
|
#define USB_BUFSIZE (5*1024)
|
|
|
|
static struct usb_device_descriptor device_desc = {
|
|
bLength : sizeof(device_desc),
|
|
bDescriptorType : USB_DT_DEVICE,
|
|
bcdUSB : __constant_cpu_to_le16(0x0200),
|
|
bDeviceClass : USB_CLASS_VENDOR_SPEC,
|
|
bDeviceSubClass : 0,
|
|
bDeviceProtocol : 0,
|
|
bMaxPacketSize0 : 0,
|
|
idVendor : __constant_cpu_to_le16(DRIVER_VENDOR_NUM),
|
|
idProduct : __constant_cpu_to_le16(DRIVER_PRODUCT_NUM),
|
|
bcdDevice : __constant_cpu_to_le16(0x0107),
|
|
iManufacturer : STRINGID_MFGR,
|
|
iProduct : STRINGID_PRODUCT,
|
|
iSerialNumber : STRINGID_SERIAL,
|
|
bNumConfigurations : 1,
|
|
};
|
|
|
|
#define MAX_USB_POWER 1
|
|
|
|
static const struct usb_config_descriptor config = {
|
|
bLength : sizeof(config),
|
|
bDescriptorType : USB_DT_CONFIG,
|
|
wTotalLength : 0,
|
|
bNumInterfaces : 1,
|
|
bConfigurationValue : 3,
|
|
iConfiguration : 0,
|
|
bmAttributes : (USB_CONFIG_ATT_ONE | USB_CONFIG_ATT_SELFPOWER),
|
|
bMaxPower : ((MAX_USB_POWER + 1) / 2),
|
|
};
|
|
|
|
static const struct usb_interface_descriptor bulk_intf = {
|
|
bLength : sizeof(bulk_intf),
|
|
bDescriptorType : USB_DT_INTERFACE,
|
|
bInterfaceNumber : 0,
|
|
bAlternateSetting : 0,
|
|
bNumEndpoints : 2,
|
|
bInterfaceClass : USB_CLASS_VENDOR_SPEC,
|
|
bInterfaceSubClass : 0,
|
|
bInterfaceProtocol : 0,
|
|
iInterface : STRINGID_INTERFACE,
|
|
};
|
|
|
|
static struct usb_endpoint_descriptor fs_source_desc = {
|
|
bLength : USB_DT_ENDPOINT_SIZE,
|
|
bDescriptorType : USB_DT_ENDPOINT,
|
|
bEndpointAddress : USB_DIR_IN | 1,
|
|
bmAttributes : USB_ENDPOINT_XFER_BULK,
|
|
wMaxPacketSize : __constant_cpu_to_le16(64),
|
|
bInterval : 0,
|
|
bRefresh : 0,
|
|
bSynchAddress : 0,
|
|
};
|
|
|
|
static struct usb_endpoint_descriptor fs_sink_desc = {
|
|
bLength : USB_DT_ENDPOINT_SIZE,
|
|
bDescriptorType : USB_DT_ENDPOINT,
|
|
bEndpointAddress : USB_DIR_OUT | 2,
|
|
bmAttributes : USB_ENDPOINT_XFER_BULK,
|
|
wMaxPacketSize : __constant_cpu_to_le16(64),
|
|
bInterval : 0,
|
|
bRefresh : 0,
|
|
bSynchAddress : 0,
|
|
};
|
|
|
|
static const struct usb_endpoint_descriptor *fs_eps[2] = {
|
|
&fs_source_desc,
|
|
&fs_sink_desc,
|
|
};
|
|
|
|
static struct usb_endpoint_descriptor hs_source_desc = {
|
|
bLength : USB_DT_ENDPOINT_SIZE,
|
|
bDescriptorType : USB_DT_ENDPOINT,
|
|
bEndpointAddress : USB_DIR_IN | 1,
|
|
bmAttributes : USB_ENDPOINT_XFER_BULK,
|
|
wMaxPacketSize : __constant_cpu_to_le16(512),
|
|
bInterval : 0,
|
|
bRefresh : 0,
|
|
bSynchAddress : 0,
|
|
};
|
|
|
|
static struct usb_endpoint_descriptor hs_sink_desc = {
|
|
bLength : USB_DT_ENDPOINT_SIZE,
|
|
bDescriptorType : USB_DT_ENDPOINT,
|
|
bEndpointAddress : USB_DIR_OUT | 2,
|
|
bmAttributes : USB_ENDPOINT_XFER_BULK,
|
|
wMaxPacketSize : __constant_cpu_to_le16(512),
|
|
bInterval : 0,
|
|
bRefresh : 0,
|
|
bSynchAddress : 0,
|
|
};
|
|
|
|
static const struct usb_endpoint_descriptor *hs_eps[2] = {
|
|
&hs_source_desc,
|
|
&hs_sink_desc,
|
|
};
|
|
|
|
static char serial[64];
|
|
|
|
static struct usb_string stringtab[] = {
|
|
{ STRINGID_MFGR, "Critical Link, LLC", },
|
|
{ STRINGID_PRODUCT, "MityCAM", },
|
|
{ STRINGID_SERIAL, serial, },
|
|
{ STRINGID_CONFIG, "Config", },
|
|
{ STRINGID_INTERFACE, "Bulk Transfer", },
|
|
};
|
|
|
|
static struct usb_gadget_strings strings = {
|
|
language : 0x0409, /* en-us */
|
|
strings : stringtab,
|
|
};
|
|
|
|
}
|
|
|
|
/**
|
|
* Gadget FS Based USB peripheral driver code.
|
|
*
|
|
* \param[in] apParser pointer to valid parser for data handling
|
|
* \param[in] anSerial the camera serial number
|
|
* \param[in] apRe pointer to valid report error class
|
|
*/
|
|
tcUSB::tcUSB(tcDspParserBase* apParser, int anSerial, tcReportError* apRe)
|
|
: mnEP0Fd(-1)
|
|
, mnBulkInFd(-1)
|
|
, mnBulkOutFd(-1)
|
|
, mpParser(apParser)
|
|
, mhEP0Thread(pthread_self())
|
|
, mhBulkThread(pthread_self())
|
|
, mhStartupThread(pthread_self())
|
|
, mnBytesIn(0)
|
|
, mpReportError(apRe)
|
|
, mbConnected(false)
|
|
{
|
|
sprintf(serial, "%d", anSerial);
|
|
}
|
|
|
|
/**
|
|
* Interface destructor
|
|
*/
|
|
tcUSB::~tcUSB(void)
|
|
{
|
|
if (mnEP0Fd >= 0)
|
|
close(mnEP0Fd);
|
|
}
|
|
|
|
char* tcUSB::build_config(char* cp,
|
|
const struct usb_endpoint_descriptor **ep)
|
|
{
|
|
struct usb_config_descriptor *c;
|
|
int i;
|
|
|
|
c = (struct usb_config_descriptor *)cp;
|
|
|
|
memcpy(cp, &config, config.bLength);
|
|
cp += config.bLength;
|
|
|
|
memcpy(cp, &bulk_intf, bulk_intf.bLength);
|
|
cp += bulk_intf.bLength;
|
|
|
|
for (i = 0; i < bulk_intf.bNumEndpoints; i++)
|
|
{
|
|
memcpy(cp, ep[i], USB_DT_ENDPOINT_SIZE);
|
|
cp += USB_DT_ENDPOINT_SIZE;
|
|
}
|
|
c->wTotalLength = __cpu_to_le16(cp - (char*)c);
|
|
return cp;
|
|
}
|
|
|
|
int tcUSB::Initialize(void)
|
|
{
|
|
char buf[4096], *cp = &buf[0];
|
|
int rv, status;
|
|
|
|
mnEP0Fd = open("/dev/gadgetfs/musb-hdrc", O_RDWR);
|
|
if (mnEP0Fd < 0)
|
|
{
|
|
return mnEP0Fd;
|
|
}
|
|
mpReportError->Report("usb device opened\n");
|
|
|
|
*(__u32 *)cp = 0;
|
|
cp += 4;
|
|
|
|
/* write full speed configs */
|
|
cp = build_config(cp, fs_eps);
|
|
|
|
/* write high speed configs */
|
|
cp = build_config(cp, hs_eps);
|
|
|
|
/* write device descriptor */
|
|
memcpy(cp, &device_desc, sizeof(device_desc));
|
|
cp += sizeof(device_desc);
|
|
|
|
rv = write(mnEP0Fd, &buf[0], cp - buf);
|
|
if (rv < 0)
|
|
{
|
|
mpReportError->Report("initial write error : %s\n", strerror(errno));
|
|
close(mnEP0Fd);
|
|
mnEP0Fd = -1;
|
|
return mnEP0Fd;
|
|
}
|
|
else if (rv != (cp - buf))
|
|
{
|
|
mpReportError->Report("rv mismatch : %d, %d\n", rv, (cp-buf));
|
|
close(mnEP0Fd);
|
|
mnEP0Fd = -2;
|
|
return mnEP0Fd;
|
|
}
|
|
|
|
|
|
/* spawn the control endpoint reader thread */
|
|
pthread_attr_t attr;
|
|
rv = pthread_attr_init(&attr);
|
|
|
|
status = pthread_create(&mhEP0Thread, &attr, DispatchEP0Handler, (void*)this);
|
|
if (status)
|
|
{
|
|
mpReportError->Report("pthread_create (network poll thread) : %s\n", strerror(errno));
|
|
return status;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int tcUSB::StopIO(void)
|
|
{
|
|
if (!pthread_equal(mhBulkThread, mhStartupThread))
|
|
{
|
|
pthread_cancel(mhBulkThread);
|
|
if (pthread_join(mhBulkThread, 0) != 0)
|
|
{
|
|
mpReportError->Report("Could not stop Bulk Thread");
|
|
}
|
|
else
|
|
{
|
|
close(mnBulkOutFd);
|
|
mnBulkOutFd = -1;
|
|
}
|
|
mhBulkThread = mhStartupThread;
|
|
}
|
|
if (mnBulkInFd >= 0)
|
|
{
|
|
close(mnBulkInFd);
|
|
mnBulkInFd = -1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int tcUSB::StartIO(void)
|
|
{
|
|
int status, rv;
|
|
uint32_t bufi[256/4];
|
|
char* buf = (char*)&bufi[0];
|
|
pthread_attr_t attr;
|
|
|
|
rv = pthread_attr_init(&attr);
|
|
|
|
/* open up the bulk input endpoint (relative to PC)
|
|
* file descriptor for sending data via put() call */
|
|
mnBulkInFd = open("/dev/gadgetfs/ep1in", O_WRONLY);
|
|
if (mnBulkInFd < 0)
|
|
{
|
|
mpReportError->Report("Could not open ep1in (%s)\n",
|
|
strerror(errno));
|
|
return -errno;
|
|
}
|
|
|
|
/* write full speed descriptor in */
|
|
*(uint32_t *)buf = 1;
|
|
memcpy(buf + 4, &fs_source_desc, USB_DT_ENDPOINT_SIZE);
|
|
|
|
/* write high speed descriptor in */
|
|
memcpy(buf + 4 + USB_DT_ENDPOINT_SIZE,
|
|
&hs_source_desc, USB_DT_ENDPOINT_SIZE);
|
|
|
|
/* push it to control framework */
|
|
if (write(mnBulkInFd, buf, 4 + 2 * USB_DT_ENDPOINT_SIZE) < 0)
|
|
{
|
|
mpReportError->Report("Could not setup bulkin descriptors "
|
|
" (%s)\n", strerror(errno));
|
|
close(mnBulkInFd);
|
|
return -errno;
|
|
}
|
|
|
|
/* open up the bulk output endpoint (relative to PC)
|
|
* file descriptor for receiving app data */
|
|
mnBulkOutFd = open("/dev/gadgetfs/ep2out", O_RDWR);
|
|
if (mnBulkOutFd < 0)
|
|
{
|
|
mpReportError->Report("Could not open ep2out (%s)\n",
|
|
strerror(errno));
|
|
close(mnBulkInFd);
|
|
return -errno;
|
|
}
|
|
|
|
/* write full speed descriptor in */
|
|
*(uint32_t *)buf = 1;
|
|
memcpy(buf + 4, &fs_sink_desc, USB_DT_ENDPOINT_SIZE);
|
|
|
|
/* write high speed descriptor in */
|
|
memcpy(buf + 4 + USB_DT_ENDPOINT_SIZE,
|
|
&hs_sink_desc, USB_DT_ENDPOINT_SIZE);
|
|
|
|
/* push it to control framework */
|
|
if (write(mnBulkOutFd, buf, 4 + 2 * USB_DT_ENDPOINT_SIZE) < 0)
|
|
{
|
|
mpReportError->Report("Could not setup bulkout descriptors "
|
|
" (%s)\n", strerror(errno));
|
|
close(mnBulkInFd);
|
|
close(mnBulkOutFd);
|
|
return -errno;
|
|
}
|
|
|
|
/* spawn the bulk endpoint reader thread */
|
|
status = pthread_create(&mhBulkThread, &attr, DispatchBulkInHandler, (void*)this);
|
|
if (status)
|
|
{
|
|
mpReportError->Report("pthread_create (usb poll thread) : %s\n", strerror(errno));
|
|
}
|
|
|
|
return status;
|
|
}
|
|
|
|
void* tcUSB::DispatchEP0Handler(void *apThis)
|
|
{
|
|
tcUSB *lpUSB = (tcUSB *)apThis;
|
|
lpUSB->EP0Handler();
|
|
return NULL;
|
|
}
|
|
|
|
#define USB_REQ_GET_STATE 0xDF
|
|
#define USB_REQ_CLEAR_INKTEND 0xDF
|
|
#define USB_REQ_PEEK_BUFF 0xDD
|
|
#define USB_REQ_POKE_BUFF 0xDC
|
|
#define USB_REQ_WRITE_SN 0xDB
|
|
#define USB_REQ_READ_SN 0xDA
|
|
|
|
int tcUSB::HandleControl(struct usb_ctrlrequest *setup)
|
|
{
|
|
uint16_t value, index, length;
|
|
uint8_t buf[256];
|
|
int status, tmp;
|
|
|
|
value = __le16_to_cpu(setup->wValue);
|
|
index = __le16_to_cpu(setup->wIndex);
|
|
length = __le16_to_cpu(setup->wLength);
|
|
|
|
#if 0
|
|
printf("Handle Control : %02x.%02x v%04x i%04x %d\n",
|
|
setup->bRequestType, setup->bRequest,
|
|
value, index, length);
|
|
#endif
|
|
|
|
switch(setup->bRequest)
|
|
{
|
|
case USB_REQ_GET_DESCRIPTOR:
|
|
switch(value >> 8)
|
|
{
|
|
case USB_DT_STRING:
|
|
tmp = value & 0xFF;
|
|
if (tmp || index != strings.language)
|
|
goto stall;
|
|
status = usb_gadget_get_string(&strings, tmp, buf);
|
|
if (status < 0)
|
|
goto stall;
|
|
tmp = status;
|
|
if (length < tmp)
|
|
tmp = length;
|
|
write(mnEP0Fd, buf, tmp);
|
|
break;
|
|
default:
|
|
goto stall;
|
|
}
|
|
return 0;
|
|
|
|
case USB_REQ_SET_CONFIGURATION:
|
|
if (setup->bRequestType != USB_DIR_OUT)
|
|
goto stall;
|
|
if (value)
|
|
{
|
|
StartIO();
|
|
}
|
|
else
|
|
{
|
|
StopIO();
|
|
}
|
|
status = read(mnEP0Fd, &status, 0);
|
|
if (status)
|
|
mpReportError->Report("ack SET_CONFIGURATION\n");
|
|
return 0;
|
|
|
|
case USB_REQ_GET_INTERFACE:
|
|
return 0;
|
|
|
|
case USB_REQ_SET_INTERFACE:
|
|
return 0;
|
|
|
|
case USB_REQ_GET_STATE:
|
|
for (int i = 0; i < 20; ++i)
|
|
{
|
|
buf[i] = 0x01;
|
|
}
|
|
status = write(mnEP0Fd, buf, 20);
|
|
if (status < 0)
|
|
goto stall;
|
|
return 0;
|
|
|
|
case USB_REQ_PEEK_BUFF:
|
|
for (int i = 0; i < 48; ++i)
|
|
{
|
|
buf[i] = 0x00;
|
|
}
|
|
status = write(mnEP0Fd, buf, 48);
|
|
if (status < 0)
|
|
goto stall;
|
|
return 0;
|
|
|
|
case USB_REQ_POKE_BUFF:
|
|
status = read(mnEP0Fd, &status, 0);
|
|
return 0;
|
|
|
|
default:
|
|
goto stall;
|
|
}
|
|
|
|
stall:
|
|
if (setup->bRequestType & USB_DIR_IN)
|
|
status = read(mnEP0Fd, &status, 0);
|
|
else
|
|
status = write(mnEP0Fd, &status, 0);
|
|
|
|
if (errno != EL2HLT)
|
|
mpReportError->Report("ep0 stall");
|
|
|
|
return 0;
|
|
}
|
|
|
|
int tcUSB::EP0Handler(void)
|
|
{
|
|
struct pollfd ep0_poll;
|
|
|
|
ep0_poll.fd = mnEP0Fd;
|
|
ep0_poll.events = POLLIN | POLLOUT | POLLHUP;
|
|
|
|
while (true)
|
|
{
|
|
int tmp, i, nevent;
|
|
struct usb_gadgetfs_event event[8];
|
|
|
|
/* TODO - infinite timeout? */
|
|
tmp = poll(&ep0_poll, 1, -1);
|
|
if (tmp < 0)
|
|
{
|
|
mpReportError->Report("%s: poll : %s", __func__, strerror(errno));
|
|
break;
|
|
}
|
|
|
|
tmp = read(mnEP0Fd, &event, sizeof(event));
|
|
if (tmp < 0)
|
|
{
|
|
if (errno == EAGAIN)
|
|
{
|
|
continue;
|
|
}
|
|
mpReportError->Report("ep0 read after poll : %s\n", strerror(errno));
|
|
goto done;
|
|
}
|
|
nevent = tmp / sizeof(event[0]);
|
|
for(i = 0; i < nevent; ++i)
|
|
{
|
|
switch(event[i].type)
|
|
{
|
|
case GADGETFS_NOP:
|
|
break;
|
|
case GADGETFS_CONNECT:
|
|
mbConnected = true;
|
|
break;
|
|
case GADGETFS_SETUP:
|
|
mbConnected = true;
|
|
HandleControl(&event[i].u.setup);
|
|
break;
|
|
case GADGETFS_DISCONNECT:
|
|
mbConnected = false;
|
|
StopIO();
|
|
break;
|
|
case GADGETFS_SUSPEND:
|
|
break;
|
|
default:
|
|
mpReportError->Report("unhandled USB event %d\n", event[i].type);
|
|
break;
|
|
}
|
|
}
|
|
continue;
|
|
done:
|
|
if (mbConnected)
|
|
StopIO();
|
|
break;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
void* tcUSB::DispatchBulkInHandler(void *apThis)
|
|
{
|
|
tcUSB *lpUSB = (tcUSB *)apThis;
|
|
lpUSB->BulkInHandler();
|
|
return NULL;
|
|
}
|
|
|
|
int tcUSB::BulkInHandler(void)
|
|
{
|
|
int status;
|
|
char buf[USB_BUFSIZE];
|
|
|
|
while(true)
|
|
{
|
|
status = read(mnBulkOutFd, buf, sizeof(buf));
|
|
if (status < 0)
|
|
{
|
|
break;
|
|
}
|
|
if (status) {
|
|
mnBytesIn += status;
|
|
mpParser->AddData(buf, status);
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
#define USEAIO
|
|
|
|
#ifdef USEAIO
|
|
inline int io_setup(unsigned nr, aio_context_t *ctxp)
|
|
{
|
|
return syscall(__NR_io_setup, nr, ctxp);
|
|
}
|
|
|
|
inline int io_destroy(aio_context_t ctx)
|
|
{
|
|
return syscall(__NR_io_destroy, ctx);
|
|
}
|
|
|
|
inline int io_submit(aio_context_t ctx, long nr, struct iocb **iocbpp)
|
|
{
|
|
return syscall(__NR_io_submit, ctx, nr, iocbpp);
|
|
}
|
|
|
|
inline int io_getevents(aio_context_t ctx, long min_nr, long max_nr,
|
|
struct io_event *events, struct timespect *timeout)
|
|
{
|
|
return syscall(__NR_io_getevents, ctx, min_nr, max_nr, events, timeout);
|
|
}
|
|
|
|
#define NUM_BUFFERS 8
|
|
aio_context_t ctx;
|
|
struct iocb iocb[NUM_BUFFERS];
|
|
static int pingpong = 0;
|
|
struct io_event events[NUM_BUFFERS];
|
|
char *buffers[NUM_BUFFERS];
|
|
#endif
|
|
|
|
int bufpos = 0;
|
|
#define BUFFER_SIZE (64*1024)
|
|
char buffer[BUFFER_SIZE];
|
|
|
|
/**
|
|
* Send data via Bulk transfer output endpoint.
|
|
*/
|
|
int tcUSB::put(const char* data, int length, int flush)
|
|
{
|
|
int rv, bytes_sent = 0;
|
|
int index;
|
|
if (length <= 0)
|
|
{
|
|
/* printf("Length was %d\n", length); */
|
|
return 0;
|
|
}
|
|
if (mbConnected && mnBytesIn && (mnBulkInFd >= 0))
|
|
{
|
|
#ifndef USEAIO
|
|
if (bufpos + length > BUFFER_SIZE)
|
|
{
|
|
rv = write(mnBulkInFd, buffer, bufpos);
|
|
bufpos = 0;
|
|
}
|
|
memcpy(&buffer[bufpos], data, length);
|
|
bufpos += length;
|
|
if (flush) {
|
|
rv = write(mnBulkInFd, &buffer[0], bufpos);
|
|
bufpos = 0;
|
|
}
|
|
#else
|
|
if (0 == pingpong && !bufpos)
|
|
{
|
|
for (int i = 0; i < NUM_BUFFERS; ++i)
|
|
buffers[i] = (char*)malloc(BUFFER_SIZE);
|
|
|
|
rv = io_setup(NUM_BUFFERS+1, &ctx);
|
|
if (rv < 0)
|
|
{
|
|
perror("io_setup");
|
|
return -1;
|
|
}
|
|
}
|
|
index = pingpong;
|
|
index %= NUM_BUFFERS;
|
|
|
|
int recoverbufs = 0; /* number of buffers we will transfer */
|
|
int first_buf_size = 0;
|
|
|
|
/* if we can't fit this packet in our current buffer, advance to next
|
|
* and note fact by increasing buffer transfer count */
|
|
if (bufpos + length > BUFFER_SIZE)
|
|
{
|
|
recoverbufs++;
|
|
index++;
|
|
index %= NUM_BUFFERS;
|
|
first_buf_size = bufpos;
|
|
bufpos = 0;
|
|
}
|
|
/* if we have to flush, then we must mark this packet to transfer */
|
|
if (flush)
|
|
recoverbufs++;
|
|
|
|
/* if we need to transfer and all our buffers are outstanding, then
|
|
* wait for some to recover */
|
|
if (recoverbufs && (pingpong+recoverbufs > NUM_BUFFERS))
|
|
{
|
|
rv = io_getevents(ctx, recoverbufs, recoverbufs, events, NULL);
|
|
if (rv < 0)
|
|
{
|
|
printf("io_getevents : %d\n", rv);
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
/* if two transfers send first packet */
|
|
if (first_buf_size)
|
|
{
|
|
int tmp = index-1;
|
|
if (tmp < 0) tmp = NUM_BUFFERS-1;
|
|
memset(&iocb[tmp],0,sizeof(iocb[tmp]));
|
|
iocb[tmp].aio_fildes = mnBulkInFd;
|
|
iocb[tmp].aio_lio_opcode = IOCB_CMD_PWRITE;
|
|
iocb[tmp].aio_buf = (__u64)buffers[tmp];
|
|
iocb[tmp].aio_nbytes = first_buf_size;
|
|
iocb[tmp].aio_offset = 0;
|
|
struct iocb *cbs[1];
|
|
cbs[0] = &iocb[tmp];
|
|
rv = io_submit(ctx, 1, cbs);
|
|
if (rv != 1)
|
|
{
|
|
perror("io_submit\n");
|
|
return -1;
|
|
}
|
|
}
|
|
/* copy packet data in... */
|
|
memcpy(&buffers[index][bufpos], data, length);
|
|
bufpos += length;
|
|
|
|
/* if we need to transfer this packet, send it out */
|
|
if (flush)
|
|
{
|
|
memset(&iocb[index],0,sizeof(iocb[index]));
|
|
iocb[index].aio_fildes = mnBulkInFd;
|
|
iocb[index].aio_lio_opcode = IOCB_CMD_PWRITE;
|
|
iocb[index].aio_buf = (__u64)buffers[index];
|
|
iocb[index].aio_nbytes = bufpos;
|
|
iocb[index].aio_offset = 0;
|
|
struct iocb *cbs[1];
|
|
cbs[0] = &iocb[index];
|
|
bufpos = 0;
|
|
rv = io_submit(ctx, 1, cbs);
|
|
if (rv != 1)
|
|
{
|
|
perror("io_submit\n");
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
bytes_sent = length;
|
|
pingpong += recoverbufs;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
return -1;
|
|
}
|
|
return bytes_sent;
|
|
}
|
|
|