static int send_reply_devlist(int sockfd)
{
int ret;
struct usbip_exported_device *edev;
struct op_devlist_reply reply;
reply.ndev = 0;
/* how many devices are exported ? */
dlist_for_each_data(stub_driver->edev_list, edev, struct usbip_exported_device) {
reply.ndev += 1;
}
dbg("%d devices are exported", reply.ndev);
ret = usbip_send_op_common(sockfd, OP_REP_DEVLIST, ST_OK);
if (ret < 0) {
err("send op_common");
return ret;
}
PACK_OP_DEVLIST_REPLY(1, &reply);
ret = usbip_send(sockfd, (void *) &reply, sizeof(reply));
if (ret < 0) {
err("send op_devlist_reply");
return ret;
}
dlist_for_each_data(stub_driver->edev_list, edev, struct usbip_exported_device) {
struct usb_device pdu_udev;
dump_usb_device(&edev->udev);
memcpy(&pdu_udev, &edev->udev, sizeof(pdu_udev));
pack_usb_device(1, &pdu_udev);
ret = usbip_send(sockfd, (void *) &pdu_udev, sizeof(pdu_udev));
if (ret < 0) {
err("send pdu_udev");
return ret;
}
for (int i=0; i < edev->udev.bNumInterfaces; i++) {
struct usb_interface pdu_uinf;
dump_usb_interface(&edev->uinf[i]);
memcpy(&pdu_uinf, &edev->uinf[i], sizeof(pdu_uinf));
pack_usb_interface(1, &pdu_uinf);
ret = usbip_send(sockfd, (void *) &pdu_uinf, sizeof(pdu_uinf));
if (ret < 0) {
err("send pdu_uinf");
return ret;
}
}
}
return 0;
}
/* call at unbound state */
static int bind_to_usbip(char *busid)
{
int fd;
struct op_export_request req;
uint16_t code = OP_REP_EXPORT;
int ret;
fd = tcp_connect("127.0.0.1", USBIP_PORT_STRING);
if(fd < 0){
err("connect");
return -1;
}
ret = usbip_send_op_common(fd, OP_REQ_EXPORT, 0);
if (ret < 0) {
close(fd);
err("send op_common");
return -1;
}
snprintf(req.udev.busid, sizeof(req.udev.busid), "%s", busid);
ret = usbip_send(fd, &req, sizeof(req));
if(ret!=sizeof(req)){
close(fd);
err("send");
return -1;
}
ret = usbip_recv_op_common(fd, &code);
close(fd);
if(ret<0){
err("recv reply");
return -1;
}
return 0;
}
static int query_import_device(int sockfd, char *busid)
{
int ret;
struct op_import_request request;
struct op_import_reply reply;
uint16_t code = OP_REP_IMPORT;
bzero(&request, sizeof(request));
bzero(&reply, sizeof(reply));
/* send a request */
ret = usbip_send_op_common(sockfd, OP_REQ_IMPORT, 0);
if (ret < 0) {
err("send op_common");
return -1;
}
strncpy(request.busid, busid, SYSFS_BUS_ID_SIZE-1);
PACK_OP_IMPORT_REQUEST(0, &request);
ret = usbip_send(sockfd, (void *) &request, sizeof(request));
if (ret < 0) {
err("send op_import_request");
return -1;
}
/* recieve a reply */
ret = usbip_recv_op_common(sockfd, &code);
if (ret < 0) {
err("recv op_common");
return -1;
}
ret = usbip_recv(sockfd, (void *) &reply, sizeof(reply));
if (ret < 0) {
err("recv op_import_reply");
return -1;
}
PACK_OP_IMPORT_REPLY(0, &reply);
/* check the reply */
if (strncmp(reply.udev.busid, busid, SYSFS_BUS_ID_SIZE)) {
err("recv different busid %s", reply.udev.busid);
return -1;
}
/* import a device */
return import_device(sockfd, &reply.udev);
}
static int query_import_device(int sockfd, char *busid,
struct usb_interface *uinf0, HANDLE * fd)
{
int ret;
struct op_import_request request;
struct op_import_reply reply;
uint16_t code = OP_REP_IMPORT;
memset(&request, 0, sizeof(request));
memset(&reply, 0, sizeof(reply));
/* send a request */
ret = usbip_send_op_common(sockfd, OP_REQ_IMPORT, 0);
if (ret < 0) {
err("send op_common");
return -1;
}
strncpy(request.busid, busid, sizeof(request.busid));
request.busid[sizeof(request.busid)-1]=0;
PACK_OP_IMPORT_REQUEST(0, &request);
ret = usbip_send(sockfd, (void *) &request, sizeof(request));
if (ret < 0) {
err("send op_import_request");
return -1;
}
/* recieve a reply */
ret = usbip_recv_op_common(sockfd, &code);
if (ret < 0) {
err("recv op_common");
return -1;
}
ret = usbip_recv(sockfd, (void *) &reply, sizeof(reply));
if (ret < 0) {
err("recv op_import_reply");
return -1;
}
PACK_OP_IMPORT_REPLY(0, &reply);
/* check the reply */
if (strncmp(reply.udev.busid, busid, sizeof(reply.udev.busid))) {
err("recv different busid %s", reply.udev.busid);
return -1;
}
/* import a device */
return import_device(sockfd, &reply.udev, uinf0, fd);
}
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 send_request_unexport(int sockfd, struct usb_device *udev) {
int ret;
struct usb_device pdu_udev;
memcpy( &pdu_udev, udev, sizeof(pdu_udev) );
pack_usb_device(1, &pdu_udev);
dbg("sending usb device: (-) %u %u %u %u",
sockfd, pdu_udev.busnum, pdu_udev.devnum, pdu_udev.speed );
ret = usbip_send(sockfd, (void*)&pdu_udev, sizeof(pdu_udev));
if( ret < 0 ) {
err( "sending deviceinfo failed" );
return -1;
}
return 0;
}
int usbip_send_op_common(int sockfd, uint32_t code, uint32_t status)
{
int ret;
struct op_common op_common;
bzero(&op_common, sizeof(op_common));
op_common.version = USBIP_VERSION;
op_common.code = code;
op_common.status = status;
PACK_OP_COMMON(1, &op_common);
ret = usbip_send(sockfd, (void *) &op_common, sizeof(op_common));
if (ret < 0) {
err("send op_common");
return -1;
}
return 0;
}
static int recv_request_import(int sockfd)
{
int ret;
struct op_import_request req;
struct op_common reply;
struct usbip_exported_device *edev;
int found = 0;
int error = 0;
bzero(&req, sizeof(req));
bzero(&reply, sizeof(reply));
ret = usbip_recv(sockfd, (void *) &req, sizeof(req));
if (ret < 0) {
err("recv import request");
return -1;
}
PACK_OP_IMPORT_REQUEST(0, &req);
dlist_for_each_data(stub_driver->edev_list, edev, struct usbip_exported_device) {
if (!strncmp(req.busid, edev->udev.busid, SYSFS_BUS_ID_SIZE)) {
dbg("found requested device %s", req.busid);
found = 1;
break;
}
}
if (found) {
/* should set TCP_NODELAY for usbip */
usbip_set_nodelay(sockfd);
/* export_device needs a TCP/IP socket descriptor */
ret = usbip_stub_export_device(edev, sockfd);
if (ret < 0)
error = 1;
} else {
info("not found requested device %s", req.busid);
error = 1;
}
ret = usbip_send_op_common(sockfd, OP_REP_IMPORT, (!error ? ST_OK : ST_NA));
if (ret < 0) {
err("send import reply");
return -1;
}
if (!error) {
struct usb_device pdu_udev;
memcpy(&pdu_udev, &edev->udev, sizeof(pdu_udev));
pack_usb_device(1, &pdu_udev);
ret = usbip_send(sockfd, (void *) &pdu_udev, sizeof(pdu_udev));
if (ret < 0) {
err("send devinfo");
return -1;
}
}
return 0;
}
static int handle_device_list(const u8_t *desc, int connfd)
{
struct op_common header = {
.version = htons(USBIP_VERSION),
.code = htons(OP_REP_DEVLIST),
.status = 0,
};
LOG_DBG("desc %p", desc);
if (send(connfd, &header, sizeof(header), 0) != sizeof(header)) {
LOG_ERR("send() header failed: %s", strerror(errno));
return errno;
}
/* Send number of devices */
u32_t ndev = htonl(1);
if (send(connfd, &ndev, sizeof(ndev), 0) != sizeof(ndev)) {
LOG_ERR("send() ndev failed: %s", strerror(errno));
return errno;
}
send_device(desc, connfd);
send_interfaces(desc, connfd);
return 0;
}
static void handle_usbip_submit(int connfd, struct usbip_header *hdr)
{
struct usbip_submit *req = &hdr->u.submit;
int read;
LOG_DBG("");
read = recv(connfd, req, sizeof(*req), 0);
if (read != sizeof(*req)) {
LOG_ERR("recv() failed: %s", strerror(errno));
return;
}
usbip_header_dump((void *)hdr);
if (ntohl(hdr->common.ep) == 0) {
handle_usb_control(hdr);
} else {
handle_usb_data(hdr);
}
}
static void handle_usbip_unlink(int connfd, struct usbip_header *hdr)
{
struct usbip_unlink *req = &hdr->u.unlink;
u64_t setup_padding;
int read;
LOG_DBG("");
read = recv(connfd, req, sizeof(hdr->u), 0);
if (read != sizeof(hdr->u)) {
LOG_ERR("recv() failed: %s", strerror(errno));
return;
}
/* Read also padding */
read = recv(connfd, &setup_padding, sizeof(setup_padding), 0);
if (read != sizeof(setup_padding)) {
LOG_ERR("recv() failed: %s", strerror(errno));
return;
}
usbip_header_dump((void *)hdr);
/* TODO: unlink */
}
static int handle_import(const u8_t *desc, int connfd)
{
struct op_common header = {
.version = htons(USBIP_VERSION),
.code = htons(OP_REP_IMPORT),
.status = 0,
};
char busid[32];
LOG_DBG("attach device");
if (recv(connfd, busid, 32, 0) != sizeof(busid)) {
LOG_ERR("recv() failed: %s", strerror(errno));
return errno;
}
if (send(connfd, &header, sizeof(header), 0) != sizeof(header)) {
LOG_ERR("send() header failed: %s", strerror(errno));
return errno;
}
send_device(desc, connfd);
return 0;
}
extern struct usb_desc_header __usb_descriptor_start[];
void usbip_start(void)
{
struct sockaddr_in srv;
unsigned char attached;
int listenfd, connfd;
const u8_t *desc;
int reuse = 1;
LOG_DBG("Starting");
/*
* Do not use usb_get_device_descriptor();
* to prevent double string fixing
*/
desc = (const u8_t *)__usb_descriptor_start;
if (!desc) {
LOG_ERR("Descriptors are not set");
posix_exit(EXIT_FAILURE);
}
listenfd = socket(PF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0);
if (listenfd < 0) {
LOG_ERR("socket() failed: %s", strerror(errno));
posix_exit(EXIT_FAILURE);
}
if (setsockopt(listenfd, SOL_SOCKET, SO_REUSEADDR,
(const char *)&reuse, sizeof(reuse)) < 0) {
LOG_WRN("setsockopt() failed: %s", strerror(errno));
}
memset(&srv, 0, sizeof(srv));
srv.sin_family = AF_INET;
srv.sin_addr.s_addr = htonl(INADDR_ANY);
srv.sin_port = htons(USBIP_PORT);
if (bind(listenfd, (struct sockaddr *)&srv, sizeof(srv)) < 0) {
LOG_ERR("bind() failed: %s", strerror(errno));
posix_exit(EXIT_FAILURE);
}
if (listen(listenfd, SOMAXCONN) < 0) {
LOG_ERR("listen() failed: %s", strerror(errno));
posix_exit(EXIT_FAILURE);
}
while (true) {
struct sockaddr_in client_addr;
socklen_t client_addr_len = sizeof(client_addr);
connfd = accept4(listenfd, (struct sockaddr *)&client_addr,
&client_addr_len, SOCK_NONBLOCK);
if (connfd < 0) {
if (errno == EAGAIN || errno == EWOULDBLOCK) {
/* Non-blocking accept */
k_sleep(100);
continue;
}
LOG_ERR("accept() failed: %s", strerror(errno));
posix_exit(EXIT_FAILURE);
}
connfd_global = connfd;
LOG_DBG("Connection: %s", inet_ntoa(client_addr.sin_addr));
/* Set attached 0 */
attached = 0U;
while (true) {
struct usbip_header cmd;
struct usbip_header_common *hdr = &cmd.common;
int read;
if (!attached) {
struct op_common req;
read = recv(connfd, &req, sizeof(req), 0);
if (read < 0) {
if (errno == EAGAIN ||
errno == EWOULDBLOCK) {
/* Non-blocking accept */
k_sleep(100);
continue;
}
}
if (read != sizeof(req)) {
LOG_WRN("wrong length, %d", read);
/* Closing connection */
break;
}
LOG_HEXDUMP_DBG((u8_t *)&req, sizeof(req),
"Got request");
LOG_DBG("Code: 0x%x", ntohs(req.code));
switch (ntohs(req.code)) {
case OP_REQ_DEVLIST:
handle_device_list(desc, connfd);
break;
case OP_REQ_IMPORT:
if (!handle_import(desc, connfd)) {
attached = 1U;
}
break;
default:
LOG_ERR("Unhandled code: 0x%x",
ntohs(req.code));
break;
}
continue;
}
/* Handle attached case */
read = recv(connfd, hdr, sizeof(*hdr), 0);
if (read < 0) {
if (errno == EAGAIN || errno == EWOULDBLOCK) {
/* Non-blocking accept */
k_sleep(100);
continue;
}
}
LOG_HEXDUMP_DBG((u8_t *)hdr, read, "Got cmd");
if (read != sizeof(*hdr)) {
LOG_ERR("recv wrong length: %d", read);
/* Closing connection */
break;
}
devid_global = ntohl(hdr->devid);
seqnum_global = ntohl(hdr->seqnum);
switch (ntohl(hdr->command)) {
case USBIP_CMD_SUBMIT:
handle_usbip_submit(connfd, &cmd);
break;
case USBIP_CMD_UNLINK:
handle_usbip_unlink(connfd, &cmd);
break;
default:
LOG_ERR("Unknown command: 0x%x",
ntohl(hdr->command));
close(connfd);
return;
}
}
LOG_DBG("Closing connection");
close(connfd);
}
}
int usbip_recv(u8_t *buf, size_t len)
{
return recv(connfd_global, buf, len, 0);
}
int usbip_send(u8_t ep, const u8_t *data, size_t len)
{
return send(connfd_global, data, len, 0);
}
int usbip_send_common(u8_t ep, u32_t data_len)
{
struct usbip_submit_rsp rsp;
rsp.common.command = htonl(USBIP_RET_SUBMIT);
rsp.common.seqnum = htonl(seqnum_global);
rsp.common.devid = htonl(0);
rsp.common.direction = htonl(0); /* TODO get from ep */
rsp.common.ep = htonl(ep);
rsp.status = htonl(0);
rsp.actual_length = htonl(data_len);
rsp.start_frame = htonl(0);
rsp.number_of_packets = htonl(0);
rsp.error_count = htonl(0);
rsp.setup = htonl(0);
return usbip_send(ep, (u8_t *)&rsp, sizeof(rsp));
}
