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 dev->speed = htonl(2);
131
132 dev->idVendor = htons(dev_dsc->idVendor);
133 dev->idProduct = htons(dev_dsc->idProduct);
134 dev->bcdDevice = htons(dev_dsc->bcdDevice);
135 dev->bDeviceClass = dev_dsc->bDeviceClass;
136 dev->bDeviceSubClass = dev_dsc->bDeviceSubClass;
137 dev->bDeviceProtocol = dev_dsc->bDeviceProtocol;
138
139 dev->bConfigurationValue = cfg->bConfigurationValue;
140 dev->bNumConfigurations = dev_dsc->bNumConfigurations;
141 dev->bNumInterfaces = cfg->bNumInterfaces;
142 }
143
send_device(const uint8_t * desc,int connfd)144 static int send_device(const uint8_t *desc, int connfd)
145 {
146 struct devlist_device dev;
147
148 fill_device(&dev, desc);
149
150 if (send(connfd, &dev, sizeof(dev), 0) != sizeof(dev)) {
151 LOG_ERR("send() device failed: %s", strerror(errno));
152 return errno;
153 }
154
155 return 0;
156 }
157
handle_device_list(const uint8_t * desc,int connfd)158 static int handle_device_list(const uint8_t *desc, int connfd)
159 {
160 struct op_common header = {
161 .version = htons(USBIP_VERSION),
162 .code = htons(OP_REP_DEVLIST),
163 .status = 0,
164 };
165
166 LOG_DBG("desc %p", desc);
167
168 if (send(connfd, &header, sizeof(header), 0) != sizeof(header)) {
169 LOG_ERR("send() header failed: %s", strerror(errno));
170 return errno;
171 }
172
173 /* Send number of devices */
174 uint32_t ndev = htonl(1);
175
176 if (send(connfd, &ndev, sizeof(ndev), 0) != sizeof(ndev)) {
177 LOG_ERR("send() ndev failed: %s", strerror(errno));
178 return errno;
179 }
180
181 send_device(desc, connfd);
182
183 send_interfaces(desc, connfd);
184
185 return 0;
186 }
187
handle_usbip_submit(int connfd,struct usbip_header * hdr)188 static void handle_usbip_submit(int connfd, struct usbip_header *hdr)
189 {
190 struct usbip_submit *req = &hdr->u.submit;
191 int read;
192
193 LOG_DBG("");
194
195 read = recv(connfd, req, sizeof(*req), 0);
196 if (read != sizeof(*req)) {
197 LOG_ERR("recv() failed: %s", strerror(errno));
198 return;
199 }
200
201 usbip_header_dump((void *)hdr);
202
203 if (ntohl(hdr->common.ep) == 0) {
204 handle_usb_control(hdr);
205 } else {
206 handle_usb_data(hdr);
207 }
208 }
209
handle_usbip_unlink(int connfd,struct usbip_header * hdr)210 static void handle_usbip_unlink(int connfd, struct usbip_header *hdr)
211 {
212 int read;
213
214 LOG_DBG("");
215
216 /* Need to read the whole structure */
217 read = recv(connfd, &hdr->u, sizeof(hdr->u), 0);
218 if (read != sizeof(hdr->u)) {
219 LOG_ERR("recv() failed: %s", strerror(errno));
220 return;
221 }
222
223 usbip_header_dump((void *)hdr);
224
225 /* TODO: unlink */
226 }
227
handle_import(const uint8_t * desc,int connfd)228 static int handle_import(const uint8_t *desc, int connfd)
229 {
230 struct op_common header = {
231 .version = htons(USBIP_VERSION),
232 .code = htons(OP_REP_IMPORT),
233 .status = 0,
234 };
235 char busid[32];
236
237 LOG_DBG("attach device");
238
239 if (recv(connfd, busid, 32, 0) != sizeof(busid)) {
240 LOG_ERR("recv() failed: %s", strerror(errno));
241 return errno;
242 }
243
244 if (send(connfd, &header, sizeof(header), 0) != sizeof(header)) {
245 LOG_ERR("send() header failed: %s", strerror(errno));
246 return errno;
247 }
248
249 send_device(desc, connfd);
250
251 return 0;
252 }
253
254 extern struct usb_desc_header __usb_descriptor_start[];
255
usbip_start(void)256 void usbip_start(void)
257 {
258 struct sockaddr_in srv;
259 unsigned char attached;
260 int listenfd, connfd;
261 const uint8_t *desc;
262 int reuse = 1;
263
264 LOG_DBG("Starting");
265
266 /*
267 * Do not use usb_get_device_descriptor();
268 * to prevent double string fixing
269 */
270 desc = (const uint8_t *)__usb_descriptor_start;
271 if (!desc) {
272 LOG_ERR("Descriptors are not set");
273 posix_exit(EXIT_FAILURE);
274 }
275
276 listenfd = socket(PF_INET, SOCK_STREAM | SOCK_NONBLOCK, 0);
277 if (listenfd < 0) {
278 LOG_ERR("socket() failed: %s", strerror(errno));
279 posix_exit(EXIT_FAILURE);
280 }
281
282 if (setsockopt(listenfd, SOL_SOCKET, SO_REUSEADDR,
283 (const char *)&reuse, sizeof(reuse)) < 0) {
284 LOG_WRN("setsockopt() failed: %s", strerror(errno));
285 }
286
287 memset(&srv, 0, sizeof(srv));
288 srv.sin_family = AF_INET;
289 srv.sin_addr.s_addr = htonl(INADDR_ANY);
290 srv.sin_port = htons(USBIP_PORT);
291
292 if (bind(listenfd, (struct sockaddr *)&srv, sizeof(srv)) < 0) {
293 LOG_ERR("bind() failed: %s", strerror(errno));
294 posix_exit(EXIT_FAILURE);
295 }
296
297 if (listen(listenfd, SOMAXCONN) < 0) {
298 LOG_ERR("listen() failed: %s", strerror(errno));
299 posix_exit(EXIT_FAILURE);
300 }
301
302 while (true) {
303 struct sockaddr_in client_addr;
304 socklen_t client_addr_len = sizeof(client_addr);
305
306 connfd = accept4(listenfd, (struct sockaddr *)&client_addr,
307 &client_addr_len, SOCK_NONBLOCK);
308 if (connfd < 0) {
309 if (errno == EAGAIN || errno == EWOULDBLOCK) {
310 /* Non-blocking accept */
311 k_sleep(K_MSEC(100));
312
313 continue;
314 }
315
316 LOG_ERR("accept() failed: %s", strerror(errno));
317 posix_exit(EXIT_FAILURE);
318 }
319
320 connfd_global = connfd;
321
322 LOG_DBG("Connection: %s", inet_ntoa(client_addr.sin_addr));
323
324 /* Set attached 0 */
325 attached = 0U;
326
327 while (true) {
328 struct usbip_header cmd;
329 struct usbip_header_common *hdr = &cmd.common;
330 int read;
331
332 if (!attached) {
333 struct op_common req;
334
335 read = recv(connfd, &req, sizeof(req), 0);
336 if (read < 0) {
337 if (errno == EAGAIN ||
338 errno == EWOULDBLOCK) {
339 /* Non-blocking accept */
340 k_sleep(K_MSEC(100));
341
342 continue;
343 }
344 }
345
346 if (read != sizeof(req)) {
347 LOG_WRN("wrong length, %d", read);
348
349 /* Closing connection */
350 break;
351 }
352
353 LOG_HEXDUMP_DBG((uint8_t *)&req, sizeof(req),
354 "Got request");
355
356 LOG_DBG("Code: 0x%x", ntohs(req.code));
357
358 switch (ntohs(req.code)) {
359 case OP_REQ_DEVLIST:
360 handle_device_list(desc, connfd);
361 break;
362 case OP_REQ_IMPORT:
363 if (!handle_import(desc, connfd)) {
364 attached = 1U;
365 }
366 break;
367 default:
368 LOG_ERR("Unhandled code: 0x%x",
369 ntohs(req.code));
370 break;
371 }
372
373 continue;
374 }
375
376 /* Handle attached case */
377
378 read = recv(connfd, hdr, sizeof(*hdr), 0);
379 if (read < 0) {
380 if (errno == EAGAIN || errno == EWOULDBLOCK) {
381 /* Non-blocking accept */
382 k_sleep(K_MSEC(100));
383
384 continue;
385 }
386 }
387
388 LOG_HEXDUMP_DBG((uint8_t *)hdr, read, "Got cmd");
389
390 if (read != sizeof(*hdr)) {
391 LOG_ERR("recv wrong length: %d", read);
392
393 /* Closing connection */
394 break;
395 }
396
397 devid_global = ntohl(hdr->devid);
398 seqnum_global = ntohl(hdr->seqnum);
399
400 switch (ntohl(hdr->command)) {
401 case USBIP_CMD_SUBMIT:
402 handle_usbip_submit(connfd, &cmd);
403 break;
404 case USBIP_CMD_UNLINK:
405 handle_usbip_unlink(connfd, &cmd);
406 break;
407 default:
408 LOG_ERR("Unknown command: 0x%x",
409 ntohl(hdr->command));
410 close(connfd);
411 return;
412 }
413 }
414
415 LOG_DBG("Closing connection");
416 close(connfd);
417 }
418 }
419
usbip_recv(uint8_t * buf,size_t len)420 int usbip_recv(uint8_t *buf, size_t len)
421 {
422 return recv(connfd_global, buf, len, 0);
423 }
424
usbip_send(uint8_t ep,const uint8_t * data,size_t len)425 int usbip_send(uint8_t ep, const uint8_t *data, size_t len)
426 {
427 return send(connfd_global, data, len, 0);
428 }
429
usbip_send_common(uint8_t ep,uint32_t data_len)430 bool usbip_send_common(uint8_t ep, uint32_t data_len)
431 {
432 struct usbip_submit_rsp rsp;
433 uint32_t ep_dir = USB_EP_DIR_IS_IN(ep) ? USBIP_DIR_IN : USBIP_DIR_OUT;
434 uint32_t ep_idx = USB_EP_GET_IDX(ep);
435
436 rsp.common.command = htonl(USBIP_RET_SUBMIT);
437 rsp.common.seqnum = htonl(seqnum_global);
438 rsp.common.devid = htonl(0);
439 rsp.common.direction = htonl(ep_dir);
440 rsp.common.ep = htonl(ep_idx);
441
442 rsp.status = htonl(0);
443 rsp.actual_length = htonl(data_len);
444 rsp.start_frame = htonl(0);
445 rsp.number_of_packets = htonl(0);
446 rsp.error_count = htonl(0);
447
448 rsp.setup = htonl(0);
449
450 if (usbip_send(ep, (uint8_t *)&rsp, sizeof(rsp)) == sizeof(rsp)) {
451 return true;
452 }
453
454 return false;
455 }
456
