/* recv a pdu */
static void stub_rx_pdu(struct usbip_device *ud)
{
int ret;
struct usbip_header pdu;
struct stub_device *sdev = container_of(ud, struct stub_device, ud);
dbg_stub_rx("Enter\n");
memset(&pdu, 0, sizeof(pdu));
/* 1. receive a pdu header */
dbg_stub_rx("before usbip_xmit\n");
ret = usbip_xmit(0, ud->tcp_socket, (char *) &pdu, sizeof(pdu), 0);
dbg_stub_rx("after usbip_xmit\n");
if (ret != sizeof(pdu)) {
uerr("recv a header, %d\n", ret);
usbip_event_add(ud, SDEV_EVENT_ERROR_TCP);
return;
}
usbip_header_correct_endian(&pdu, 0);
if (dbg_flag_stub_rx)
usbip_dump_header(&pdu);
if (!valid_request(sdev, &pdu)) {
uerr("recv invalid request\n");
usbip_event_add(ud, SDEV_EVENT_ERROR_TCP);
return;
}
#if CLIENT > 2
sdev->heartbeat_pending = 0;
#endif
switch (pdu.base.command) {
case USBIP_CMD_UNLINK:
stub_recv_cmd_unlink(sdev, &pdu);
break;
case USBIP_CMD_SUBMIT:
stub_recv_cmd_submit(sdev, &pdu);
break;
#if CLIENT > 2
// added by tf, 110530
case USBIP_HEARTBEAT_REPLY:
sdev->heartbeat_pending = 0;
break;
// end --- added
#endif
default:
/* NOTREACHED */
uerr("unknown pdu\n");
usbip_event_add(ud, SDEV_EVENT_ERROR_TCP);
return;
}
}
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;
}
/* recv a pdu */
static void stub_rx_pdu(struct usbip_device *ud)
{
int ret;
struct usbip_header pdu;
struct stub_device *sdev = container_of(ud, struct stub_device, ud);
dbg_stub_rx("Enter\n");
memset(&pdu, 0, sizeof(pdu));
/* 1. receive a pdu header */
ret = usbip_xmit(0, ud->tcp_socket, (char *) &pdu, sizeof(pdu),0);
if (ret != sizeof(pdu)) {
uerr("recv a header, %d\n", ret);
usbip_event_add(ud, SDEV_EVENT_ERROR_TCP);
return;
}
usbip_header_correct_endian(&pdu, 0);
if (dbg_flag_stub_rx)
usbip_dump_header(&pdu);
if (!valid_request(sdev, &pdu)) {
uerr("recv invalid request\n");
usbip_event_add(ud, SDEV_EVENT_ERROR_TCP);
return;
}
switch (pdu.base.command) {
case USBIP_CMD_UNLINK:
stub_recv_cmd_unlink(sdev, &pdu);
break;
case USBIP_CMD_SUBMIT:
stub_recv_cmd_submit(sdev, &pdu);
break;
default:
/* NOTREACHED */
uerr("unknown pdu\n");
usbip_event_add(ud, SDEV_EVENT_ERROR_TCP);
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;
}
static void stub_recv_cmd_submit(struct stub_device *sdev, struct usbip_header *pdu)
{
int ret;
struct stub_priv *priv;
struct usbip_device *ud = &sdev->ud;
struct usb_device *udev = interface_to_usbdev(sdev->interface);
int pipe = get_pipe(sdev, pdu->base.ep, pdu->base.direction);
priv = stub_priv_alloc(sdev, pdu);
if (!priv)
return;
/* setup a urb */
if (usb_pipeisoc(pipe))
priv->urb = usb_alloc_urb(pdu->u.cmd_submit.number_of_packets, GFP_KERNEL);
else
priv->urb = usb_alloc_urb(0, GFP_KERNEL);
if (!priv->urb) {
uerr("malloc urb\n");
usbip_event_add(ud, SDEV_EVENT_ERROR_MALLOC);
return;
}
/* set priv->urb->transfer_buffer */
if (pdu->u.cmd_submit.transfer_buffer_length > 0) {
priv->urb->transfer_buffer =
kzalloc(pdu->u.cmd_submit.transfer_buffer_length, GFP_KERNEL);
if (!priv->urb->transfer_buffer) {
uerr("malloc x_buff\n");
usbip_event_add(ud, SDEV_EVENT_ERROR_MALLOC);
return;
}
}
/* set priv->urb->setup_packet */
priv->urb->setup_packet = kzalloc(8, GFP_KERNEL);
if (!priv->urb->setup_packet) {
uerr("allocate setup_packet\n");
usbip_event_add(ud, SDEV_EVENT_ERROR_MALLOC);
return;
}
memcpy(priv->urb->setup_packet, &pdu->u.cmd_submit.setup, 8);
/* set other members from the base header of pdu */
priv->urb->context = (void *) priv;
priv->urb->dev = udev;
priv->urb->pipe = pipe;
priv->urb->complete = stub_complete;
usbip_pack_pdu(pdu, priv->urb, USBIP_CMD_SUBMIT, 0);
if (usbip_recv_xbuff(ud, priv->urb) < 0)
return;
if (usbip_recv_iso(ud, priv->urb) < 0)
return;
/* no need to submit an intercepted request, but harmless? */
tweak_special_requests(priv->urb);
/* urb is now ready to submit */
ret = usb_submit_urb(priv->urb, GFP_KERNEL);
if (ret == 0)
dbg_stub_rx("submit urb ok, seqnum %u\n", pdu->base.seqnum);
else {
uerr("submit_urb error, %d\n", ret);
usbip_dump_header(pdu);
usbip_dump_urb(priv->urb);
/*
* Pessimistic.
* This connection will be discarded.
*/
usbip_event_add(ud, SDEV_EVENT_ERROR_SUBMIT);
}
dbg_stub_rx("Leave\n");
return;
}