int sock_read_async(SOCKET sockfd, HANDLE devfd, OVERLAPPED *ov_sock,
OVERLAPPED *ov_dev)
{
int ret, err=0;
unsigned long len;
do {
ret = ReadFile((HANDLE)sockfd, sock_read_buf,
sizeof(struct usbip_header), &len, ov_sock);
if (!ret)
err=GetLastError();
if(err==ERROR_IO_PENDING)
return 0;
if(err) {
err("read:%d err:%d\n",ret, err);
return -1;
}
if (len!=sizeof(struct usbip_header))
{
err=GetLastError();
err("incomplete header %d %d\n",ret,err);
}
dbg_file("Bytes read from socket synchronously: %d\n",len);
ret = write_to_dev(sock_read_buf, BIG_SIZE, len,
sockfd, devfd, ov_dev);
if(ret<0)
return -1;
}while(1);
}
void usbip_dump_buffer(unsigned char *buff, int bufflen)
{
int i,j;
char linebuf[80];
int pos=0;
for (i=0; i<bufflen; i+=16)
{
pos+=sprintf(linebuf+pos,"%8i: ",i);
for (j=i; j<i+16; j++)
{
if (j<bufflen)
pos+=sprintf(linebuf+pos,"%02X ",(int)(buff)[j]);
else
pos+=sprintf(linebuf+pos," ");
}
for (j=i; j<i+16; j++)
{
if (j<bufflen)
pos+=sprintf(linebuf+pos,"%c",(buff[j]>=32&&buff[j]<128)?((char*)buff)[j]:'.');
else
pos+=sprintf(linebuf+pos," ");
}
pos+=sprintf(linebuf+pos,"\n");
dbg_file("%s",linebuf);
// printk(KERN_DEBUG "%s",linebuf);
pos=0;
}
}
int write_to_sock(char *buf, int len, SOCKET sockfd)
{
struct usbip_header *u;
int ret;
unsigned long out_len, iso_len;
u=(struct usbip_header *)buf;
if(len<sizeof(*u)){
err("read dev len: %d\n", len);
return -1;
}
if(!u->base.direction)
out_len=ntohl(u->u.cmd_submit.transfer_buffer_length);
else
out_len=0;
if(u->u.cmd_submit.number_of_packets)
iso_len=sizeof(struct usbip_iso_packet_descriptor)*
ntohl(u->u.cmd_submit.number_of_packets);
else
iso_len=0;
if(len!= sizeof(*u) + out_len + iso_len){
err("read dev len:%d out_len:%ld"
"iso_len: %ld\n",
len, out_len, iso_len);
return -1;
}
if(!u->base.direction&&!record_out(u->base.seqnum)){
err("out q full");
return -1;
}
dbg_file("send seq:%d\r", ntohl(u->base.seqnum));
// fprintf(stderr,"Send sequence: %d\n", ntohl(u->base.seqnum));
ret=usbip_send(sockfd, buf, len);
if(ret!=len){
err("send sock len:%d, ret:%d\n", len, ret);
return -1;
}
#ifdef DEBUG
{
struct usbip_header cu;
memcpy(&cu,u,sizeof(struct usbip_header));
usbip_header_correct_endian(&cu,0);
usbip_dump_header(&cu);
}
#endif
return 0;
}
int sock_read_completed(SOCKET sockfd, HANDLE devfd, OVERLAPPED *ov_sock,
OVERLAPPED *ov_dev)
{
int ret;
unsigned long len;
ret = GetOverlappedResult((HANDLE)sockfd, ov_sock, &len, FALSE);
if(!ret){
err("get overlapping failed: %ld", GetLastError());
return -1;
}
dbg_file("Bytes read from socket asynchronously: %d\n",len);
ret = write_to_dev(sock_read_buf, BIG_SIZE, len, sockfd, devfd, ov_dev);
if(ret<0)
return -1;
return sock_read_async(sockfd, devfd, ov_sock, ov_dev);
}
static ssize_t usbip_xmit(int sockfd, void *buff, size_t bufflen, int sending)
{
ssize_t total = 0;
#ifdef DEBUG
void * orgbuf=buff;
#endif
if (!bufflen)
return 0;
// dbg_file("do %d: len:%d\n", sending, bufflen);
do {
ssize_t nbytes;
if (sending)
{
nbytes = send(sockfd, buff, bufflen, 0);
}
else
{
nbytes = recv(sockfd, buff, bufflen, 0);
dbg_file("Number of bytes received from socket synchronously: %d\n",nbytes);
}
if (nbytes <= 0)
return -1;
buff = (void *)((char *)buff + nbytes);
bufflen -= nbytes;
total += nbytes;
} while (bufflen > 0);
#ifdef DEBUG
usbip_dump_buffer(orgbuf,total);
#endif
// dbg_file("do %d: len:%d finish\n", sending, bufflen);
return total;
}
static void usbip_dump_header(struct usbip_header *pdu)
{
dbg_file("BASE: cmd %u seq %u devid %u dir %u ep %u\n",
pdu->base.command,
pdu->base.seqnum,
pdu->base.devid,
pdu->base.direction,
pdu->base.ep);
switch(pdu->base.command) {
case USBIP_CMD_SUBMIT:
dbg_file("CMD_SUBMIT: x_flags %u x_len %u sf %u #p %u iv %u\n",
pdu->u.cmd_submit.transfer_flags,
pdu->u.cmd_submit.transfer_buffer_length,
pdu->u.cmd_submit.start_frame,
pdu->u.cmd_submit.number_of_packets,
pdu->u.cmd_submit.interval);
break;
case USBIP_CMD_UNLINK:
dbg_file("CMD_UNLINK: seq %u\n", pdu->u.cmd_unlink.seqnum);
break;
case USBIP_RET_SUBMIT:
dbg_file("RET_SUBMIT: st %d al %u sf %d #p %d ec %d\n",
pdu->u.ret_submit.status,
pdu->u.ret_submit.actual_length,
pdu->u.ret_submit.start_frame,
pdu->u.cmd_submit.number_of_packets,
pdu->u.ret_submit.error_count);
break;
case USBIP_RET_UNLINK:
dbg_file("RET_UNLINK: status %d\n", pdu->u.ret_unlink.status);
break;
default:
/* NOT REACHED */
dbg_file("UNKNOWN\n");
}
}
void usbip_vbus_forward(SOCKET sockfd, HANDLE devfd)
{
HANDLE ev[3];
OVERLAPPED ov[3];
int ret;
int i;
int err=0;
dev_read_buf = malloc(BIG_SIZE);
sock_read_buf = malloc(BIG_SIZE);
if(dev_read_buf == NULL||sock_read_buf==NULL){
err("cannot allocate buffers");
return;
}
for(i=0;i<3;i++){
ev[i]=CreateEvent(NULL, FALSE, FALSE, NULL);
if(NULL==ev[i]){
err("cannot create new events");
return;
}
ov[i].Offset=ov[i].OffsetHigh=0;
ov[i].hEvent=ev[i];
}
signal(SIGINT,signalhandler);
dev_read_async(devfd, sockfd, &ov[0]);
sock_read_async(sockfd, devfd, &ov[1], &ov[2]);
do {
dbg_file("wait\n");
ret = WaitForMultipleObjects(2, ev, FALSE, 100);
// dbg_file("wait out %d\n", ret);
switch (ret) {
case WAIT_TIMEOUT:
// do nothing just give CTRL-C a chance to be detected
break;
case WAIT_OBJECT_0:
err=dev_read_completed(devfd, sockfd, &ov[0]);
break;
case WAIT_OBJECT_0 + 1:
err=sock_read_completed(sockfd, devfd, &ov[1], &ov[2]);
break;
default:
err("unknown ret %d\n",ret);
err=ret;
break;
}
} while(err==0&&!signalflag);
info("\n");
if (signalflag)
{
info("CTRL-C received\n");
}
free(dev_read_buf);
free(sock_read_buf);
return;
}
int write_to_dev(char * buf, int buf_len, int len, SOCKET sockfd,
HANDLE devfd, OVERLAPPED *ov)
{
int ret;
unsigned long out=0, in_len, iso_len;
struct usbip_header * u = (struct usbip_header *)buf;
if(len!=sizeof(*u)){
err("read from sock ret %d not equal a usbip_header", len);
#ifdef DEBUG
usbip_dump_buffer(buf,len);
#endif
return -1;
}
if(usbip_header_correct_endian(u, 0)<0)
return -1;
dbg_file("recv seq %d\n", u->base.seqnum);
if ((u->base.seqnum%100)==0)
fprintf(stderr,"Receive sequence: %d\r", u->base.seqnum);
#ifdef DEBUG
usbip_dump_header(u);
#endif
if(check_out(htonl(u->base.seqnum)))
in_len=0;
else
in_len=u->u.ret_submit.actual_length;
iso_len = u->u.ret_submit.number_of_packets
* sizeof(struct usbip_iso_packet_descriptor);
if(in_len==0&&iso_len==0){
ret=WriteFile(devfd, (char *)u, sizeof(*u), &out, ov);
if(!ret||out!=sizeof(*u)){
err("last error:%ld",GetLastError());
err("out:%ld ret:%d",out,ret);
err("write dev failed");
return -1;
}
return 0;
}
len = sizeof(*u) + in_len + iso_len;
if(len>buf_len){
err("too big len %d %ld %ld", len, in_len,iso_len);
return -1;
}
ret=usbip_recv(sockfd, buf+sizeof(*u),
in_len+iso_len);
if(ret != in_len + iso_len){
err("recv from sock failed %d %ld",
ret,
in_len + iso_len);
return -1;
}
if(iso_len)
fix_iso_desc_endian(sock_read_buf+sizeof(*u)+in_len,
u->u.ret_submit.number_of_packets);
ret=WriteFile(devfd, buf, len, &out, ov);
if(!ret||out!=len){
err("last error:%ld\n",GetLastError());
err("out:%ld ret:%d len:%d\n",out,ret,len);
err("write dev failed");
return -1;
}
return 0;
}
