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));
}
