1 /*
2 * Copyright (c) 2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <string.h>
8 #include <stdio.h>
9 #include <zephyr/kernel.h>
10 #include <zephyr/sys/byteorder.h>
11 #include <zephyr/usb/usb_ch9.h>
12 #include <zephyr/usb/usbh.h>
13 #include <zephyr/net/socket.h>
14 #include <zephyr/net_buf.h>
15
16 #include <usbh_device.h>
17 #include <usbh_ch9.h>
18 #include <usbip.h>
19
20 #include <zephyr/logging/log.h>
21 LOG_MODULE_REGISTER(usbip, CONFIG_USBIP_LOG_LEVEL);
22
23 /* For now, we will use the Zephyr default controller. */
24 USBH_CONTROLLER_DEFINE(usbip_uhs_ctx, DEVICE_DT_GET(DT_NODELABEL(zephyr_uhc0)));
25
26 #define USBIP_MAX_PKT_SIZE 2048
27 NET_BUF_POOL_DEFINE(usbip_pool, 32, USBIP_MAX_PKT_SIZE, 0, NULL);
28
29 K_THREAD_STACK_DEFINE(usbip_thread_stack, CONFIG_USBIP_THREAD_STACK_SIZE);
30 K_THREAD_STACK_ARRAY_DEFINE(dev_thread_stacks, CONFIG_USBIP_DEVICES_COUNT,
31 CONFIG_USBIP_THREAD_STACK_SIZE);
32
33 static struct k_thread usbip_thread;
34
35 #define USBIP_DEFAULT_PATH "/sys/bus/usb/devices/usb1/1-"
36
37 #define USBIP_EXPORTED BIT(0)
38
39 /* Context of the exported device */
40 struct usbip_dev_ctx {
41 struct usb_device *udev;
42 struct k_thread thread;
43 struct k_event event;
44 sys_dlist_t dlist;
45 int connfd;
46 uint32_t devid;
47 };
48
49 /* Context of the exported bus (not really used yet) */
50 struct usbip_bus_ctx {
51 struct usbh_contex *uhs_ctx;
52 struct usbip_dev_ctx devs[CONFIG_USBIP_DEVICES_COUNT];
53 uint8_t busnum;
54 };
55
56 static struct usbip_bus_ctx default_bus_ctx;
57
58 /* Command reference structure to find the way back */
59 struct usbip_cmd_node {
60 sys_dnode_t node;
61 struct usbip_command cmd;
62 struct usbip_dev_ctx *ctx;
63 struct uhc_transfer *xfer;
64 };
65
66 K_MEM_SLAB_DEFINE(usbip_slab, sizeof(struct usbip_cmd_node),
67 CONFIG_USBIP_SUBMIT_BACKLOG_COUNT, sizeof(void *));
68
usbip_ntoh_command(struct usbip_command * const cmd)69 static void usbip_ntoh_command(struct usbip_command *const cmd)
70 {
71 cmd->hdr.command = ntohl(cmd->hdr.command);
72 cmd->hdr.seqnum = ntohl(cmd->hdr.seqnum);
73 cmd->hdr.devid = ntohl(cmd->hdr.devid);
74 cmd->hdr.direction = ntohl(cmd->hdr.direction);
75 cmd->hdr.ep = ntohl(cmd->hdr.ep);
76
77 if (cmd->hdr.command == USBIP_CMD_SUBMIT) {
78 cmd->submit.flags = ntohl(cmd->submit.flags);
79 cmd->submit.length = ntohl(cmd->submit.length);
80 cmd->submit.start_frame = ntohl(cmd->submit.start_frame);
81 cmd->submit.numof_iso_pkts = ntohl(cmd->submit.numof_iso_pkts);
82 cmd->submit.interval = ntohl(cmd->submit.interval);
83 } else {
84 cmd->unlink.seqnum = ntohl(cmd->unlink.seqnum);
85 }
86 }
87
check_ctrl_request(struct usb_device * const udev,const int ep,const uint8_t * const setup_pkt)88 static void check_ctrl_request(struct usb_device *const udev,
89 const int ep, const uint8_t *const setup_pkt)
90 {
91 struct usb_setup_packet setup = {0};
92
93 memcpy(&setup, setup_pkt, sizeof(struct usb_setup_packet));
94 if (setup.RequestType.type != USB_REQTYPE_TYPE_STANDARD) {
95 return;
96 }
97
98 if (setup.RequestType.direction) {
99 return;
100 }
101
102 switch (setup.bRequest) {
103 case USB_SREQ_SET_ADDRESS:
104 LOG_INF("Set Address");
105 break;
106 case USB_SREQ_SET_CONFIGURATION:
107 LOG_INF("Set Configuration");
108 break;
109 case USB_SREQ_SET_INTERFACE:
110 LOG_INF("Set Interface");
111 if (usbh_device_interface_set(udev, setup.wIndex, setup.wValue, true)) {
112 LOG_ERR("Failed to apply Set Interface request");
113 }
114 break;
115 default:
116 break;
117 }
118 }
119
usbip_req_cb(struct usb_device * const udev,struct uhc_transfer * const xfer)120 static int usbip_req_cb(struct usb_device *const udev, struct uhc_transfer *const xfer)
121 {
122 struct usbip_cmd_node *const cmd_nd = xfer->priv;
123 struct usbip_dev_ctx *const dev_ctx = cmd_nd->ctx;
124 struct usbip_command *const cmd = &cmd_nd->cmd;
125 struct net_buf *buf = xfer->buf;
126 struct usbip_return ret;
127 unsigned int key;
128 int err;
129
130 LOG_INF("SUBMIT seqnum %u finished err %d ep 0x%02x",
131 cmd->hdr.seqnum, xfer->err, xfer->ep);
132
133 ret.hdr.command = htonl(USBIP_RET_SUBMIT);
134 ret.hdr.seqnum = htonl(cmd->hdr.seqnum);
135 ret.hdr.devid = htonl(cmd->hdr.devid);
136 ret.hdr.ep = htonl(xfer->ep);
137 ret.hdr.direction = htonl(cmd->hdr.direction);
138
139 memset(&ret.submit, 0, sizeof(ret.submit));
140 ret.submit.status = htonl(xfer->err);
141 ret.submit.start_frame = htonl(cmd->submit.start_frame);
142 ret.submit.numof_iso_pkts = htonl(0xFFFFFFFFUL);
143
144 if (xfer->err == -ECONNRESET) {
145 LOG_INF("URB seqnum %u unlinked (ECONNRESET)", cmd->hdr.seqnum);
146 goto usbip_req_cb_error;
147 }
148
149 if (xfer->err == -EPIPE) {
150 LOG_INF("RET_SUBMIT status is EPIPE");
151 }
152
153 if (xfer->err == 0 && cmd->submit.length != 0) {
154 ret.submit.actual_length = htonl(buf->len);
155 }
156
157
158 if (USB_EP_GET_IDX(xfer->ep) == 0U) {
159 check_ctrl_request(dev_ctx->udev, xfer->ep, xfer->setup_pkt);
160 }
161
162 err = zsock_send(dev_ctx->connfd, &ret, sizeof(ret), 0);
163 if (err != sizeof(ret)) {
164 LOG_ERR("Send RET_SUBMIT failed err %d errno %d", err, errno);
165 err = -errno;
166 goto usbip_req_cb_error;
167 }
168
169 if (ret.submit.actual_length != 0) {
170 LOG_INF("Send RET_SUBMIT transfer_buffer len %u", buf->len);
171 err = zsock_send(dev_ctx->connfd, buf->data, buf->len, 0);
172 if (err != buf->len) {
173 LOG_ERR("Send transfer_buffer failed err %d errno %d",
174 err, errno);
175 err = -errno;
176 goto usbip_req_cb_error;
177 }
178 }
179
180 usbip_req_cb_error:
181 key = irq_lock();
182 sys_dlist_remove(&cmd_nd->node);
183 irq_unlock(key);
184
185 k_mem_slab_free(&usbip_slab, (void *)cmd_nd);
186 if (xfer->buf) {
187 net_buf_unref(buf);
188 }
189
190 usbh_xfer_free(dev_ctx->udev, xfer);
191
192 return 0;
193 }
194
usbip_submit_req(struct usbip_cmd_node * const cmd_nd,const uint8_t ep,const struct usb_setup_packet * const setup,struct net_buf * const buf)195 static int usbip_submit_req(struct usbip_cmd_node *const cmd_nd, const uint8_t ep,
196 const struct usb_setup_packet *const setup,
197 struct net_buf *const buf)
198 {
199 struct usbip_dev_ctx *const dev_ctx = cmd_nd->ctx;
200 struct usbip_command *const cmd = &cmd_nd->cmd;
201 struct usb_device *const udev = dev_ctx->udev;
202 struct uhc_transfer *xfer;
203 int ret;
204
205 xfer = usbh_xfer_alloc(udev, ep, usbip_req_cb, cmd_nd);
206 if (xfer == NULL) {
207 return -ENOMEM;
208 }
209
210 if (setup != NULL) {
211 memcpy(xfer->setup_pkt, setup, sizeof(struct usb_setup_packet));
212 LOG_HEXDUMP_DBG(xfer->setup_pkt, 8U, "setup");
213 }
214
215 if (buf != NULL) {
216 ret = usbh_xfer_buf_add(dev_ctx->udev, xfer, buf);
217 if (ret) {
218 goto submit_req_err;
219 }
220 }
221
222 xfer->interval = cmd->submit.interval;
223 cmd_nd->xfer = xfer;
224 ret = usbh_xfer_enqueue(dev_ctx->udev, xfer);
225 if (ret) {
226 goto submit_req_err;
227 }
228
229 return 0;
230
231 submit_req_err:
232 usbh_xfer_free(dev_ctx->udev, xfer);
233
234 return ret;
235 }
236
usbip_handle_submit(struct usbip_dev_ctx * const dev_ctx,struct usbip_command * const cmd)237 static int usbip_handle_submit(struct usbip_dev_ctx *const dev_ctx,
238 struct usbip_command *const cmd)
239 {
240 struct usbip_cmd_submit *submit = &cmd->submit;
241 struct usb_setup_packet setup = {0};
242 struct usbip_cmd_node *cmd_nd;
243 struct net_buf *buf = NULL;
244 uint8_t ep;
245 int ret;
246
247 ret = zsock_recv(dev_ctx->connfd, submit, sizeof(*submit), ZSOCK_MSG_WAITALL);
248 if (ret <= 0) {
249 return ret == 0 ? -ENOTCONN : -errno;
250 }
251
252 usbip_ntoh_command(cmd);
253
254 ep = cmd->hdr.ep;
255 if (cmd->submit.length != 0) {
256 buf = net_buf_alloc(&usbip_pool, K_NO_WAIT);
257 if (buf == NULL) {
258 LOG_ERR("Failed to allocate net_buf");
259 return -ENOMEM;
260 }
261 }
262
263 if (cmd->hdr.direction == USBIP_DIR_OUT && cmd->submit.length != 0) {
264 /* Receive data */
265 ret = zsock_recv(dev_ctx->connfd,
266 buf->data, cmd->submit.length,
267 ZSOCK_MSG_WAITALL);
268 if (ret <= 0) {
269 net_buf_unref(buf);
270 return ret == 0 ? -ENOTCONN : -errno;
271 }
272
273 net_buf_add(buf, cmd->submit.length);
274 }
275
276 if (USB_EP_GET_IDX(ep) == 0U) {
277 /* Setup packet */
278 memcpy(&setup, cmd->submit.setup, sizeof(setup));
279 ep = usb_reqtype_is_to_device(&setup) ? 0x00 : 0x80;
280 } else {
281 /* Set endpoint depending on direction */
282 if (cmd->hdr.direction == USBIP_DIR_IN) {
283 ep |= USB_EP_DIR_IN;
284 }
285 }
286
287 LOG_INF("Handle SUBMIT devid %x seqnum %u length %u ep 0x%02x flags 0x%08x",
288 cmd->hdr.devid, cmd->hdr.seqnum, cmd->submit.length, ep, cmd->submit.flags);
289
290 if (k_mem_slab_alloc(&usbip_slab, (void **)&cmd_nd, K_MSEC(1000)) != 0) {
291 LOG_ERR("Failed to allocate slab");
292 net_buf_unref(buf);
293 return -ENOMEM;
294 }
295
296 /* Make a copy of the command and add it to the backlog */
297 memcpy(&cmd_nd->cmd, cmd, sizeof(struct usbip_command));
298 cmd_nd->ctx = dev_ctx;
299 sys_dlist_append(&dev_ctx->dlist, &cmd_nd->node);
300
301 ret = usbip_submit_req(cmd_nd, ep, &setup, buf);
302 if (ret != 0) {
303 LOG_ERR("Failed to submit request %d", ret);
304 return ret;
305 }
306
307 LOG_INF("Append %u ep 0x%02x to list", cmd->hdr.seqnum, ep);
308 if (cmd->submit.length != 0) {
309 LOG_HEXDUMP_DBG(buf->data, buf->len, "SUBMIT data");
310 }
311
312 return 0;
313 }
314
usbip_handle_unlink(struct usbip_dev_ctx * const dev_ctx,struct usbip_command * const cmd)315 static int usbip_handle_unlink(struct usbip_dev_ctx *const dev_ctx,
316 struct usbip_command *const cmd)
317 {
318 struct usbip_cmd_unlink *unlink = &cmd->unlink;
319 struct usbip_return rsp;
320 struct usbip_cmd_node *cmd_nd;
321 unsigned int key;
322 int ret;
323
324 ret = zsock_recv(dev_ctx->connfd, unlink, sizeof(*unlink), ZSOCK_MSG_WAITALL);
325 if (ret <= 0) {
326 return ret == 0 ? -ENOTCONN : -errno;
327 }
328
329 memcpy(&rsp.hdr, &cmd->hdr, sizeof(rsp.hdr));
330 rsp.hdr.command = htonl(USBIP_RET_UNLINK);
331
332 usbip_ntoh_command(cmd);
333
334 LOG_INF("Unlink request (seqnum %u) seqnum %u",
335 cmd->hdr.seqnum, cmd->unlink.seqnum);
336
337 memset(&rsp.unlink, 0, sizeof(rsp.unlink));
338
339 key = irq_lock();
340 SYS_DLIST_FOR_EACH_CONTAINER(&dev_ctx->dlist, cmd_nd, node) {
341 if (cmd_nd->cmd.hdr.seqnum == cmd->unlink.seqnum) {
342 rsp.unlink.status = htonl(-ECONNRESET);
343 usbh_xfer_dequeue(dev_ctx->udev, cmd_nd->xfer);
344 break;
345 }
346 }
347 irq_unlock(key);
348
349 ret = zsock_send(dev_ctx->connfd, &rsp, sizeof(rsp), 0);
350 if (ret != sizeof(rsp)) {
351 return -errno;
352 }
353
354 return 0;
355 }
356
usbip_handle_cmd(struct usbip_dev_ctx * const dev_ctx)357 static int usbip_handle_cmd(struct usbip_dev_ctx *const dev_ctx)
358 {
359 struct usbip_command cmd;
360 int ret;
361
362 ret = zsock_recv(dev_ctx->connfd, &cmd.hdr, sizeof(cmd.hdr), ZSOCK_MSG_WAITALL);
363 if (ret <= 0) {
364 return ret == 0 ? -ENOTCONN : -errno;
365 }
366
367 LOG_HEXDUMP_DBG((uint8_t *)&cmd.hdr, sizeof(cmd.hdr), "cmd.hdr");
368
369 switch (ntohl(cmd.hdr.command)) {
370 case USBIP_CMD_SUBMIT:
371 ret = usbip_handle_submit(dev_ctx, &cmd);
372 break;
373 case USBIP_CMD_UNLINK:
374 ret = usbip_handle_unlink(dev_ctx, &cmd);
375 break;
376 default:
377 LOG_ERR("Unknown command: 0x%x", ntohl(cmd.hdr.command));
378 break;
379 }
380
381 return ret;
382 }
383
usbip_thread_cmd(void * const a,void * const b,void * const c)384 static void usbip_thread_cmd(void *const a, void *const b, void *const c)
385 {
386 struct usbip_dev_ctx *dev_ctx = a;
387 int ret;
388
389 LOG_INF("CMD thread started");
390 while (true) {
391 k_event_wait(&dev_ctx->event, USBIP_EXPORTED, false, K_FOREVER);
392
393 ret = usbip_handle_cmd(dev_ctx);
394
395 if (ret) {
396 zsock_close(dev_ctx->connfd);
397 LOG_INF("CMD connection closed, errno %d", ret);
398 k_event_set_masked(&dev_ctx->event, 0, USBIP_EXPORTED);
399 dev_ctx->udev = NULL;
400 }
401 }
402 }
403
handle_devlist_device(struct usb_device * const udev,const uint32_t busnum,int connfd)404 static int handle_devlist_device(struct usb_device *const udev,
405 const uint32_t busnum, int connfd)
406 {
407 struct usb_device_descriptor *d_desc = &udev->dev_desc;
408 struct usb_cfg_descriptor *c_desc = udev->cfg_desc;
409 static struct usbip_devlist_data devlist;
410 const uint32_t devnum = udev->addr;
411 int err;
412
413 devlist.busnum = htonl(busnum);
414 devlist.devnum = htonl(devnum);
415
416 devlist.speed = htonl(udev->speed);
417 devlist.idVendor = htons(d_desc->idVendor);
418 devlist.idProduct = htons(d_desc->idProduct);
419 devlist.bcdDevice = htons(d_desc->bcdDevice);
420 devlist.bDeviceClass = d_desc->bDeviceClass;
421 devlist.bDeviceSubClass = d_desc->bDeviceSubClass;
422 devlist.bDeviceProtocol = d_desc->bDeviceProtocol;
423
424 devlist.bConfigurationValue = c_desc->bConfigurationValue;
425 devlist.bNumConfigurations = d_desc->bNumConfigurations;
426 devlist.bNumInterfaces = c_desc->bNumInterfaces;
427
428 snprintf(devlist.path, sizeof(devlist.path), USBIP_DEFAULT_PATH "%d", devnum);
429 snprintf(devlist.busid, sizeof(devlist.busid), "1-%d", devnum);
430
431 LOG_INF("bLength\t\t\t%u", c_desc->bLength);
432 LOG_INF("bDescriptorType\t\t%u", c_desc->bDescriptorType);
433 LOG_INF("wTotalLength\t\t%u", c_desc->wTotalLength);
434 LOG_INF("bNumInterfaces\t\t%u", c_desc->bNumInterfaces);
435 LOG_INF("bConfigurationValue\t%u", c_desc->bConfigurationValue);
436 LOG_INF("iConfiguration\t\t%u", c_desc->iConfiguration);
437 LOG_INF("bmAttributes\t\t%02x", c_desc->bmAttributes);
438 LOG_INF("bMaxPower\t\t%u mA", c_desc->bMaxPower * 2);
439
440 err = zsock_send(connfd, &devlist, sizeof(devlist), 0);
441 if (err != sizeof(devlist)) {
442 return -errno;
443 }
444
445 return 0;
446 }
447
handle_devlist_device_iface(struct usb_device * const udev,int connfd)448 static int handle_devlist_device_iface(struct usb_device *const udev, int connfd)
449 {
450 struct usb_cfg_descriptor *c_desc = udev->cfg_desc;
451 static struct usbip_devlist_iface_data iface;
452 int err;
453
454 LOG_INF("Handle OP_REQ_DEVLIST, bNumInterfaces %u wTotalLength %u",
455 c_desc->bNumInterfaces, c_desc->wTotalLength);
456
457 for (unsigned int i = 0; i < c_desc->bNumInterfaces; i++) {
458 struct usb_if_descriptor *if_d = (void *)udev->ifaces[i].dhp;
459
460 LOG_DBG("bInterfaceNumber %u", if_d->bInterfaceNumber);
461
462 iface.bInterfaceClass = if_d->bInterfaceClass;
463 iface.bInterfaceSubClass = if_d->bInterfaceSubClass;
464 iface.bInterfaceProtocol = if_d->bInterfaceProtocol;
465 iface.padding = 0U;
466
467 err = zsock_send(connfd, &iface, sizeof(iface), 0);
468 if (err != sizeof(iface)) {
469 LOG_ERR("Failed to send interface info %d", err);
470 return -errno;
471 }
472 }
473
474 return 0;
475 }
476
usbip_handle_devlist(struct usbip_bus_ctx * const bus_ctx,int connfd)477 static int usbip_handle_devlist(struct usbip_bus_ctx *const bus_ctx, int connfd)
478 {
479 struct usbip_devlist_header rep_hdr = {
480 .version = htons(USBIP_VERSION),
481 .code = htons(USBIP_OP_REP_DEVLIST),
482 .status = 0,
483 .ndev = htonl(1),
484 };
485 struct usb_device *udev;
486 uint32_t ndev = 0;
487 int err;
488
489 SYS_DLIST_FOR_EACH_CONTAINER(&bus_ctx->uhs_ctx->udevs, udev, node) {
490 ndev++;
491 }
492
493 rep_hdr.ndev = htonl(ndev);
494 /* Send reply header with the number of USB devices */
495 err = zsock_send(connfd, &rep_hdr, sizeof(rep_hdr), 0);
496 if (err != sizeof(rep_hdr)) {
497 return -errno;
498 }
499
500 /* Send device info for each USB devices */
501 SYS_DLIST_FOR_EACH_CONTAINER(&bus_ctx->uhs_ctx->udevs, udev, node) {
502 err = handle_devlist_device(udev, bus_ctx->busnum, connfd);
503 if (err) {
504 return err;
505 }
506
507 err = handle_devlist_device_iface(udev, connfd);
508 if (err) {
509 return err;
510 }
511 }
512
513 return 0;
514 }
515
get_device_by_busid(struct usbip_bus_ctx * const bus_ctx,const char busid[static32])516 static struct usb_device *get_device_by_busid(struct usbip_bus_ctx *const bus_ctx,
517 const char busid[static 32])
518 {
519 struct usb_device *udev;
520 char my_busid[32];
521
522 LOG_HEXDUMP_DBG(busid, sizeof(my_busid), "import busid");
523
524 SYS_DLIST_FOR_EACH_CONTAINER(&bus_ctx->uhs_ctx->udevs, udev, node) {
525 snprintf(my_busid, sizeof(my_busid), "1-%d", udev->addr);
526
527 LOG_HEXDUMP_DBG(my_busid, sizeof(my_busid), "my busid");
528 if (!strncmp(busid, my_busid, sizeof(my_busid))) {
529 return udev;
530 }
531 }
532
533 return NULL;
534 }
535
get_free_dev_ctx(struct usbip_bus_ctx * const bus_ctx)536 static struct usbip_dev_ctx *get_free_dev_ctx(struct usbip_bus_ctx *const bus_ctx)
537 {
538 for (unsigned int i = 0; i < CONFIG_USBIP_DEVICES_COUNT; i++) {
539 struct usbip_dev_ctx *const dev_ctx = &bus_ctx->devs[i];
540
541 if (k_event_wait(&dev_ctx->event, USBIP_EXPORTED, false, K_NO_WAIT)) {
542 continue;
543 }
544
545 if (dev_ctx->udev != NULL) {
546 LOG_WRN("USB device pointer is not cleaned");
547 }
548
549 return &bus_ctx->devs[i];
550 }
551
552 return NULL;
553 }
554
usbip_handle_import(struct usbip_bus_ctx * const bus_ctx,int connfd)555 static int usbip_handle_import(struct usbip_bus_ctx *const bus_ctx, int connfd)
556 {
557 struct usbip_req_header rep_hdr = {
558 .version = htons(USBIP_VERSION),
559 .code = htons(USBIP_OP_REP_IMPORT),
560 .status = 0,
561 };
562 struct usbip_dev_ctx *dev_ctx;
563 uint8_t busid[32];
564 int ret;
565
566 ret = zsock_recv(connfd, &busid, sizeof(busid), ZSOCK_MSG_WAITALL);
567 if (ret <= 0) {
568 return ret == 0 ? -ENOTCONN : -errno;
569 }
570
571 dev_ctx = get_free_dev_ctx(bus_ctx);
572 if (dev_ctx == NULL) {
573 rep_hdr.status = htonl(-1);
574 LOG_ERR("No free device context to export a device");
575 } else {
576 dev_ctx->udev = get_device_by_busid(bus_ctx, busid);
577 if (dev_ctx->udev == NULL) {
578 rep_hdr.status = htonl(-1);
579 dev_ctx = NULL;
580 LOG_ERR("No USB device with busid %s", busid);
581 }
582 }
583
584 ret = zsock_send(connfd, &rep_hdr, sizeof(rep_hdr), 0);
585 if (ret != sizeof(rep_hdr)) {
586 return -errno;
587 }
588
589 if (rep_hdr.status || dev_ctx == NULL) {
590 LOG_ERR("Device does not exits or cannot be exported");
591 return -1;
592 }
593
594 ret = handle_devlist_device(dev_ctx->udev, bus_ctx->busnum, connfd);
595 if (ret) {
596 return ret;
597 }
598
599 dev_ctx->connfd = connfd;
600 k_event_post(&dev_ctx->event, USBIP_EXPORTED);
601 LOG_INF("USB device %s exported", busid);
602
603 return 0;
604 }
605
usbip_handle_connection(struct usbip_bus_ctx * const bus_ctx,int connfd)606 static int usbip_handle_connection(struct usbip_bus_ctx *const bus_ctx, int connfd)
607 {
608 struct usbip_req_header hdr;
609 int ret;
610
611 ret = zsock_recv(connfd, &hdr, sizeof(hdr), ZSOCK_MSG_WAITALL);
612 if (ret <= 0) {
613 return ret == 0 ? -ENOTCONN : -errno;
614 }
615
616 LOG_HEXDUMP_DBG((uint8_t *)&hdr, sizeof(hdr), "header");
617 LOG_INF("Code: 0x%x", ntohs(hdr.code));
618
619 switch (ntohs(hdr.code)) {
620 case USBIP_OP_REQ_DEVLIST:
621 ret = usbip_handle_devlist(bus_ctx, connfd);
622 zsock_close(connfd);
623 break;
624 case USBIP_OP_REQ_IMPORT:
625 ret = usbip_handle_import(bus_ctx, connfd);
626 if (ret) {
627 zsock_close(connfd);
628 }
629 break;
630 default:
631 LOG_ERR("Unknown request: 0x%x", ntohs(hdr.code));
632 ret = -1;
633 break;
634 }
635
636 return ret;
637 }
638
usbip_thread_handler(void * const a,void * const b,void * const c)639 static void usbip_thread_handler(void *const a, void *const b, void *const c)
640 {
641 struct usbip_bus_ctx *const bus_ctx = a;
642 struct sockaddr_in srv;
643 int listenfd;
644 int connfd;
645 int reuse = 1;
646
647 LOG_DBG("Started connection handling thread");
648
649 listenfd = zsock_socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
650 if (listenfd < 0) {
651 LOG_ERR("socket() failed: %s", strerror(errno));
652 return;
653 }
654
655 if (zsock_setsockopt(listenfd, SOL_SOCKET, SO_REUSEADDR,
656 (const char *)&reuse, sizeof(reuse)) < 0) {
657 LOG_INF("setsockopt() failed: %s", strerror(errno));
658 }
659
660 srv.sin_family = AF_INET;
661 srv.sin_addr.s_addr = htonl(INADDR_ANY);
662 srv.sin_port = htons(USBIP_PORT);
663
664 if (zsock_bind(listenfd, (struct sockaddr *)&srv, sizeof(srv)) < 0) {
665 LOG_ERR("bind() failed: %s", strerror(errno));
666 return;
667 }
668
669 if (zsock_listen(listenfd, 1) < 0) {
670 LOG_ERR("listen() failed: %s", strerror(errno));
671 return;
672 }
673
674 while (true) {
675 struct sockaddr_in client_addr;
676 socklen_t client_addr_len = sizeof(client_addr);
677 char addr_str[INET_ADDRSTRLEN];
678 int err;
679
680 connfd = zsock_accept(listenfd, (struct sockaddr *)&client_addr,
681 &client_addr_len);
682 if (connfd < 0) {
683 LOG_ERR("accept() failed: %d", errno);
684 continue;
685 }
686
687 zsock_inet_ntop(client_addr.sin_family, &client_addr.sin_addr,
688 addr_str, sizeof(addr_str));
689 LOG_INF("Connection: %s", addr_str);
690
691 err = usbip_handle_connection(bus_ctx, connfd);
692 LOG_INF("Connection from %s closed, errno %d", addr_str, err);
693 }
694 }
695
696 /*
697 * We are just using a standard host controller, which is fine to get USBIP
698 * support working and stable, but it needs a better solution in the future.
699 */
usbip_init(void)700 static int usbip_init(void)
701 {
702 struct usbip_bus_ctx *const bus_ctx = &default_bus_ctx;
703 int err;
704
705 err = usbh_init(&usbip_uhs_ctx);
706 if (err) {
707 LOG_ERR("Failed to initialize host support");
708 return err;
709 }
710
711 err = usbh_enable(&usbip_uhs_ctx);
712 if (err) {
713 LOG_ERR("Failed to enable host support");
714 return err;
715 }
716
717 err = uhc_sof_enable(usbip_uhs_ctx.dev);
718 if (err) {
719 LOG_ERR("Failed to start SoF");
720 return err;
721 }
722
723 LOG_INF("Host controller enabled");
724 bus_ctx->uhs_ctx = &usbip_uhs_ctx;
725 bus_ctx->busnum = 1;
726
727 for (int i = 0; i < CONFIG_USBIP_DEVICES_COUNT; i++) {
728 struct usbip_dev_ctx *ctx = &bus_ctx->devs[i];
729
730 /* busnum << 16 | devnum */
731 ctx->devid = (1U << 16) | i;
732 sys_dlist_init(&ctx->dlist);
733 k_event_init(&ctx->event);
734 k_thread_create(&ctx->thread, dev_thread_stacks[i],
735 K_THREAD_STACK_SIZEOF(dev_thread_stacks[i]),
736 usbip_thread_cmd, ctx, NULL, NULL,
737 K_PRIO_COOP(3), 0, K_NO_WAIT);
738 }
739
740 k_thread_create(&usbip_thread, usbip_thread_stack,
741 K_THREAD_STACK_SIZEOF(usbip_thread_stack),
742 usbip_thread_handler, bus_ctx, NULL, NULL,
743 K_PRIO_COOP(2), 0, K_NO_WAIT);
744
745 return 0;
746 }
747
748 SYS_INIT(usbip_init, APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEVICE);
749