1 /*
2 * Copyright (c) 2018 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /* For accept4() */
8 #define _GNU_SOURCE 1
9
10 #define __packed __attribute__((__packed__))
11
12 #include <stdio.h>
13 #include <stdarg.h>
14 #include <stdbool.h>
15 #include <errno.h>
16 #include <string.h>
17 #include <unistd.h>
18 #include <stdlib.h>
19
20 #include <sys/types.h>
21 #include <sys/socket.h>
22 #include <sys/un.h>
23 #include <netinet/in.h>
24 #include <arpa/inet.h>
25
26 /* Zephyr headers */
27 #include <zephyr/kernel.h>
28 #include <zephyr/usb/usb_device.h>
29
30 #include <posix_board_if.h>
31 #include "usb_dc_native_posix_adapt.h"
32
33 #define LOG_LEVEL CONFIG_USB_DRIVER_LOG_LEVEL
34 LOG_MODULE_REGISTER(native_posix_adapt);
35
36 #define USBIP_PORT 3240
37 #define USBIP_VERSION 273
38
39 #define VERBOSE_DEBUG
40
41 int connfd_global;
42 int seqnum_global;
43 int devid_global;
44
45 /* Helpers */
46
47 #ifdef VERBOSE_DEBUG
usbip_header_dump(struct usbip_header * hdr)48 static void usbip_header_dump(struct usbip_header *hdr)
49 {
50 LOG_DBG("cmd %x seq %u dir %u ep %x", ntohl(hdr->common.command),
51 ntohl(hdr->common.seqnum), ntohl(hdr->common.direction),
52 ntohl(hdr->common.ep));
53
54 switch (ntohl(hdr->common.command)) {
55 case USBIP_CMD_SUBMIT:
56 LOG_DBG("flags %x np %u int %u buflen %u",
57 ntohl(hdr->u.submit.transfer_flags),
58 ntohl(hdr->u.submit.number_of_packets),
59 ntohl(hdr->u.submit.interval),
60 ntohl(hdr->u.submit.transfer_buffer_length));
61 break;
62 case USBIP_CMD_UNLINK:
63 LOG_DBG("seq %d", ntohl(hdr->u.unlink.seqnum));
64 break;
65 default:
66 break;
67 }
68 }
69 #else
70 #define usbip_header_dump(x)
71 #endif
72
get_interface(uint8_t * descriptors)73 void get_interface(uint8_t *descriptors)
74 {
75 while (descriptors[0]) {
76 if (descriptors[1] == USB_DESC_INTERFACE) {
77 LOG_DBG("interface found");
78 }
79
80 /* skip to next descriptor */
81 descriptors += descriptors[0];
82 }
83 }
84
send_interfaces(const uint8_t * descriptors,int connfd)85 static int send_interfaces(const uint8_t *descriptors, int connfd)
86 {
87 struct devlist_interface {
88 uint8_t bInterfaceClass;
89 uint8_t bInterfaceSubClass;
90 uint8_t bInterfaceProtocol;
91 uint8_t padding; /* alignment */
92 } __packed iface;
93
94 while (descriptors[0]) {
95 if (descriptors[1] == USB_DESC_INTERFACE) {
96 struct usb_if_descriptor *desc = (void *)descriptors;
97
98 iface.bInterfaceClass = desc->bInterfaceClass;
99 iface.bInterfaceSubClass = desc->bInterfaceSubClass;
100 iface.bInterfaceProtocol = desc->bInterfaceProtocol;
101 iface.padding = 0U;
102
103 if (send(connfd, &iface, sizeof(iface), 0) !=
104 sizeof(iface)) {
105 LOG_ERR("send() failed: %s", strerror(errno));
106 return errno;
107 }
108 }
109
110 /* skip to next descriptor */
111 descriptors += descriptors[0];
112 }
113
114 return 0;
115 }
116
fill_device(struct devlist_device * dev,const uint8_t * desc)117 static void fill_device(struct devlist_device *dev, const uint8_t *desc)
118 {
119 struct usb_device_descriptor *dev_dsc = (void *)desc;
120 struct usb_cfg_descriptor *cfg =
121 (void *)(desc + sizeof(struct usb_device_descriptor));
122
123 memset(dev->path, 0, 256);
124 strcpy(dev->path, "/sys/devices/pci0000:00/0000:00:01.2/usb1/1-1");
125 memset(dev->busid, 0, 32);
126 strcpy(dev->busid, "1-1");
127
128 dev->busnum = htonl(1);
129 dev->devnum = htonl(2);
130 if (IS_ENABLED(CONFIG_USB_NATIVE_POSIX_HS)) {
131 dev->speed = htonl(3);
132 } else {
133 dev->speed = htonl(2);
134 }
135
136 dev->idVendor = htons(dev_dsc->idVendor);
137 dev->idProduct = htons(dev_dsc->idProduct);
138 dev->bcdDevice = htons(dev_dsc->bcdDevice);
139 dev->bDeviceClass = dev_dsc->bDeviceClass;
140 dev->bDeviceSubClass = dev_dsc->bDeviceSubClass;
141 dev->bDeviceProtocol = dev_dsc->bDeviceProtocol;
142
143 dev->bConfigurationValue = cfg->bConfigurationValue;
144 dev->bNumConfigurations = dev_dsc->bNumConfigurations;
145 dev->bNumInterfaces = cfg->bNumInterfaces;
146 }
147
send_device(const uint8_t * desc,int connfd)148 static int send_device(const uint8_t *desc, int connfd)
149 {
150 struct devlist_device dev;
151
152 fill_device(&dev, desc);
153
154 if (send(connfd, &dev, sizeof(dev), 0) != sizeof(dev)) {
155 LOG_ERR("send() device failed: %s", strerror(errno));
156 return errno;
157 }
158
159 return 0;
160 }
161
handle_device_list(const uint8_t * desc,int connfd)162 static int handle_device_list(const uint8_t *desc, int connfd)
163 {
164 struct op_common header = {
165 .version = htons(USBIP_VERSION),
166 .code = htons(OP_REP_DEVLIST),
167 .status = 0,
168 };
169
170 LOG_DBG("desc %p", desc);
171
172 if (send(connfd, &header, sizeof(header), 0) != sizeof(header)) {
173 LOG_ERR("send() header failed: %s", strerror(errno));
174 return errno;
175 }
176
177 /* Send number of devices */
178 uint32_t ndev = htonl(1);
179
180 if (send(connfd, &ndev, sizeof(ndev), 0) != sizeof(ndev)) {
181 LOG_ERR("send() ndev failed: %s", strerror(errno));
182 return errno;
183 }
184
185 send_device(desc, connfd);
186
187 send_interfaces(desc, connfd);
188
189 return 0;
190 }
191
handle_usbip_submit(int connfd,struct usbip_header * hdr)192 static void handle_usbip_submit(int connfd, struct usbip_header *hdr)
193 {
194 struct usbip_submit *req = &hdr->u.submit;
195 int read;
196
197 LOG_DBG("");
198
199 read = recv(connfd, req, sizeof(*req), 0);
200 if (read != sizeof(*req)) {
201 LOG_ERR("recv() failed: %s", strerror(errno));
202 return;
203 }
204
205 usbip_header_dump((void *)hdr);
206
207 if (ntohl(hdr->common.ep) == 0) {
208 handle_usb_control(hdr);
209 } else {
210 handle_usb_data(hdr);
211 }
212 }
213
handle_usbip_unlink(int connfd,struct usbip_header * hdr)214 static void handle_usbip_unlink(int connfd, struct usbip_header *hdr)
215 {
216 int read;
217
218 LOG_DBG("");
219
220 /* Need to read the whole structure */
221 read = recv(connfd, &hdr->u, sizeof(hdr->u), 0);
222 if (read != sizeof(hdr->u)) {
223 LOG_ERR("recv() failed: %s", strerror(errno));
224 return;
225 }
226
227 usbip_header_dump((void *)hdr);
228
229 /* TODO: unlink */
230 }
231
handle_import(const uint8_t * desc,int connfd)232 static int handle_import(const uint8_t *desc, int connfd)
233 {
234 struct op_common header = {
235 .version = htons(USBIP_VERSION),
236 .code = htons(OP_REP_IMPORT),
237 .status = 0,
238 };
239 char busid[32];
240
241 LOG_DBG("attach device");
242
243 if (recv(connfd, busid, 32, 0) != sizeof(busid)) {
244 LOG_ERR("recv() failed: %s", strerror(errno));
245 return errno;
246 }
247
248 if (send(connfd, &header, sizeof(header), 0) != sizeof(header)) {
249 LOG_ERR("send() header failed: %s", strerror(errno));
250 return errno;
251 }
252
253 send_device(desc, connfd);
254
255 return 0;
256 }
257
258 extern struct usb_desc_header __usb_descriptor_start[];
259
usbip_start(void)260 void usbip_start(void)
261 {
262 struct sockaddr_in srv;
263 unsigned char attached;
264 int listenfd, connfd;
265 const uint8_t *desc;
266 int reuse = 1;
267
268 LOG_DBG("Starting");
269
270 /*
271 * Do not use usb_get_device_descriptor();
272 * to prevent double string fixing
273 */
274 desc = (const uint8_t *)__usb_descriptor_start;
275 if (!desc) {
276 LOG_ERR("Descriptors are not set");
277 posix_exit(EXIT_FAILURE);
278 }
279
280 listenfd = socket(PF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0);
281 if (listenfd < 0) {
282 LOG_ERR("socket() failed: %s", strerror(errno));
283 posix_exit(EXIT_FAILURE);
284 }
285
286 if (setsockopt(listenfd, SOL_SOCKET, SO_REUSEADDR,
287 (const char *)&reuse, sizeof(reuse)) < 0) {
288 LOG_WRN("setsockopt() failed: %s", strerror(errno));
289 }
290
291 memset(&srv, 0, sizeof(srv));
292 srv.sin_family = AF_INET;
293 srv.sin_addr.s_addr = htonl(INADDR_ANY);
294 srv.sin_port = htons(USBIP_PORT);
295
296 if (bind(listenfd, (struct sockaddr *)&srv, sizeof(srv)) < 0) {
297 LOG_ERR("bind() failed: %s", strerror(errno));
298 posix_exit(EXIT_FAILURE);
299 }
300
301 if (listen(listenfd, SOMAXCONN) < 0) {
302 LOG_ERR("listen() failed: %s", strerror(errno));
303 posix_exit(EXIT_FAILURE);
304 }
305
306 while (true) {
307 struct sockaddr_in client_addr;
308 socklen_t client_addr_len = sizeof(client_addr);
309
310 connfd = accept4(listenfd, (struct sockaddr *)&client_addr,
311 &client_addr_len, SOCK_NONBLOCK);
312 if (connfd < 0) {
313 if (errno == EAGAIN || errno == EWOULDBLOCK) {
314 /* Non-blocking accept */
315 k_sleep(K_MSEC(100));
316
317 continue;
318 }
319
320 LOG_ERR("accept() failed: %s", strerror(errno));
321 posix_exit(EXIT_FAILURE);
322 }
323
324 connfd_global = connfd;
325
326 LOG_DBG("Connection: %s", inet_ntoa(client_addr.sin_addr));
327
328 /* Set attached 0 */
329 attached = 0U;
330
331 while (true) {
332 struct usbip_header cmd;
333 struct usbip_header_common *hdr = &cmd.common;
334 int read;
335
336 if (!attached) {
337 struct op_common req;
338
339 read = recv(connfd, &req, sizeof(req), 0);
340 if (read < 0) {
341 if (errno == EAGAIN ||
342 errno == EWOULDBLOCK) {
343 /* Non-blocking accept */
344 k_sleep(K_MSEC(100));
345
346 continue;
347 }
348 }
349
350 if (read != sizeof(req)) {
351 LOG_WRN("wrong length, %d", read);
352
353 /* Closing connection */
354 break;
355 }
356
357 LOG_HEXDUMP_DBG((uint8_t *)&req, sizeof(req),
358 "Got request");
359
360 LOG_DBG("Code: 0x%x", ntohs(req.code));
361
362 switch (ntohs(req.code)) {
363 case OP_REQ_DEVLIST:
364 handle_device_list(desc, connfd);
365 break;
366 case OP_REQ_IMPORT:
367 if (!handle_import(desc, connfd)) {
368 attached = 1U;
369 }
370 break;
371 default:
372 LOG_ERR("Unhandled code: 0x%x",
373 ntohs(req.code));
374 break;
375 }
376
377 continue;
378 }
379
380 /* Handle attached case */
381
382 read = recv(connfd, hdr, sizeof(*hdr), 0);
383 if (read < 0) {
384 if (errno == EAGAIN || errno == EWOULDBLOCK) {
385 /* Non-blocking accept */
386 k_sleep(K_MSEC(100));
387
388 continue;
389 }
390 }
391
392 LOG_HEXDUMP_DBG((uint8_t *)hdr, read, "Got cmd");
393
394 if (read != sizeof(*hdr)) {
395 LOG_ERR("recv wrong length: %d", read);
396
397 /* Closing connection */
398 break;
399 }
400
401 devid_global = ntohl(hdr->devid);
402 seqnum_global = ntohl(hdr->seqnum);
403
404 switch (ntohl(hdr->command)) {
405 case USBIP_CMD_SUBMIT:
406 handle_usbip_submit(connfd, &cmd);
407 break;
408 case USBIP_CMD_UNLINK:
409 handle_usbip_unlink(connfd, &cmd);
410 break;
411 default:
412 LOG_ERR("Unknown command: 0x%x",
413 ntohl(hdr->command));
414 close(connfd);
415 return;
416 }
417 }
418
419 LOG_DBG("Closing connection");
420 close(connfd);
421 }
422 }
423
usbip_recv(uint8_t * buf,size_t len)424 int usbip_recv(uint8_t *buf, size_t len)
425 {
426 return recv(connfd_global, buf, len, 0);
427 }
428
usbip_send(uint8_t ep,const uint8_t * data,size_t len)429 int usbip_send(uint8_t ep, const uint8_t *data, size_t len)
430 {
431 return send(connfd_global, data, len, 0);
432 }
433
usbip_send_common(uint8_t ep,uint32_t data_len)434 bool usbip_send_common(uint8_t ep, uint32_t data_len)
435 {
436 struct usbip_submit_rsp rsp;
437 uint32_t ep_dir = USB_EP_DIR_IS_IN(ep) ? USBIP_DIR_IN : USBIP_DIR_OUT;
438 uint32_t ep_idx = USB_EP_GET_IDX(ep);
439
440 rsp.common.command = htonl(USBIP_RET_SUBMIT);
441 rsp.common.seqnum = htonl(seqnum_global);
442 rsp.common.devid = htonl(0);
443 rsp.common.direction = htonl(ep_dir);
444 rsp.common.ep = htonl(ep_idx);
445
446 rsp.status = htonl(0);
447 rsp.actual_length = htonl(data_len);
448 rsp.start_frame = htonl(0);
449 rsp.number_of_packets = htonl(0);
450 rsp.error_count = htonl(0);
451
452 rsp.setup = htonl(0);
453
454 if (usbip_send(ep, (uint8_t *)&rsp, sizeof(rsp)) == sizeof(rsp)) {
455 return true;
456 }
457
458 return false;
459 }
460
