void handle_usb_request(int sockfd, USBIP_RET_SUBMIT *ret, int bl) { if(ret->ep == 0) { printf("#control requests\n"); handle_usb_control(sockfd, ret); } else { printf("#data requests\n"); handle_data(sockfd, ret, bl); } };
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)); }