1 /*
2 * Copyright (c) 2017 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(usb_rndis, CONFIG_USB_DEVICE_NETWORK_LOG_LEVEL);
9
10 /* Enable verbose debug printing extra hexdumps */
11 #define VERBOSE_DEBUG 0
12
13 #include <zephyr/init.h>
14
15 #include <zephyr/net/ethernet.h>
16 #include <net_private.h>
17
18 #include <zephyr/usb/usb_device.h>
19 #include <zephyr/usb/class/usb_cdc.h>
20 #include <os_desc.h>
21
22 #include "netusb.h"
23 #include "function_rndis.h"
24
25 /* RNDIS handling */
26 #define CFG_RNDIS_TX_BUF_COUNT 5
27 #define CFG_RNDIS_TX_BUF_SIZE 512
28 NET_BUF_POOL_DEFINE(rndis_tx_pool, CFG_RNDIS_TX_BUF_COUNT,
29 CFG_RNDIS_TX_BUF_SIZE, 0, NULL);
30 static struct k_fifo rndis_tx_queue;
31
32 /* Serialize RNDIS command queue for later processing */
33 #define CFG_RNDIS_CMD_BUF_COUNT 2
34 #define CFG_RNDIS_CMD_BUF_SIZE CONFIG_USB_REQUEST_BUFFER_SIZE
35 NET_BUF_POOL_DEFINE(rndis_cmd_pool, CFG_RNDIS_CMD_BUF_COUNT,
36 CFG_RNDIS_CMD_BUF_SIZE, 0, NULL);
37 static struct k_fifo rndis_cmd_queue;
38
39 /*
40 * Stack for cmd thread
41 */
42 static K_KERNEL_STACK_DEFINE(cmd_stack, 2048);
43 static struct k_thread cmd_thread_data;
44
45 struct usb_rndis_config {
46 struct usb_association_descriptor iad;
47 struct usb_if_descriptor if0;
48 struct usb_ep_descriptor if0_int_ep;
49
50 struct usb_if_descriptor if1;
51 struct usb_ep_descriptor if1_in_ep;
52 struct usb_ep_descriptor if1_out_ep;
53 } __packed;
54
55 USBD_CLASS_DESCR_DEFINE(primary, 0) struct usb_rndis_config rndis_cfg = {
56 .iad = {
57 .bLength = sizeof(struct usb_association_descriptor),
58 .bDescriptorType = USB_DESC_INTERFACE_ASSOC,
59 .bFirstInterface = 0,
60 .bInterfaceCount = 0x02,
61 .bFunctionClass = USB_BCC_MISCELLANEOUS,
62 .bFunctionSubClass = 4,
63 .bFunctionProtocol = 1,
64 .iFunction = 0,
65 },
66 /* Interface descriptor 0 */
67 .if0 = {
68 .bLength = sizeof(struct usb_if_descriptor),
69 .bDescriptorType = USB_DESC_INTERFACE,
70 .bInterfaceNumber = 0,
71 .bAlternateSetting = 0,
72 .bNumEndpoints = 1,
73 .bInterfaceClass = USB_BCC_MISCELLANEOUS,
74 .bInterfaceSubClass = 4,
75 .bInterfaceProtocol = 1,
76 .iInterface = 0,
77 },
78 /* Notification EP Descriptor */
79 .if0_int_ep = {
80 .bLength = sizeof(struct usb_ep_descriptor),
81 .bDescriptorType = USB_DESC_ENDPOINT,
82 .bEndpointAddress = RNDIS_INT_EP_ADDR,
83 .bmAttributes = USB_DC_EP_INTERRUPT,
84 .wMaxPacketSize =
85 sys_cpu_to_le16(CONFIG_RNDIS_INTERRUPT_EP_MPS),
86 .bInterval = 0x09,
87 },
88 /* Interface descriptor 1 */
89 .if1 = {
90 .bLength = sizeof(struct usb_if_descriptor),
91 .bDescriptorType = USB_DESC_INTERFACE,
92 .bInterfaceNumber = 1,
93 .bAlternateSetting = 0,
94 .bNumEndpoints = 2,
95 .bInterfaceClass = USB_BCC_CDC_DATA,
96 .bInterfaceSubClass = 0,
97 .bInterfaceProtocol = 0,
98 .iInterface = 0,
99 },
100 /* Data Endpoint IN */
101 .if1_in_ep = {
102 .bLength = sizeof(struct usb_ep_descriptor),
103 .bDescriptorType = USB_DESC_ENDPOINT,
104 .bEndpointAddress = RNDIS_IN_EP_ADDR,
105 .bmAttributes = USB_DC_EP_BULK,
106 .wMaxPacketSize =
107 sys_cpu_to_le16(CONFIG_RNDIS_BULK_EP_MPS),
108 .bInterval = 0x00,
109 },
110 /* Data Endpoint OUT */
111 .if1_out_ep = {
112 .bLength = sizeof(struct usb_ep_descriptor),
113 .bDescriptorType = USB_DESC_ENDPOINT,
114 .bEndpointAddress = RNDIS_OUT_EP_ADDR,
115 .bmAttributes = USB_DC_EP_BULK,
116 .wMaxPacketSize =
117 sys_cpu_to_le16(CONFIG_RNDIS_BULK_EP_MPS),
118 .bInterval = 0x00,
119 },
120 };
121
122 /*
123 * TLV structure is used for data encapsulation parsing
124 */
125 struct tlv {
126 uint32_t type;
127 uint32_t len;
128 uint8_t data[];
129 } __packed;
130
131 static struct __rndis {
132 uint32_t net_filter;
133
134 enum {
135 UNINITIALIZED,
136 INITIALIZED,
137 } state;
138
139 struct net_pkt *in_pkt; /* Pointer to pkt assembling at the moment */
140 int in_pkt_len; /* Packet length to be assembled */
141 int skip_bytes; /* In case of low memory, skip bytes */
142
143 uint16_t mtu;
144 uint16_t speed; /* TODO: Calculate right speed */
145
146 /* Statistics */
147 uint32_t rx_err;
148 uint32_t tx_err;
149 uint32_t rx_no_buf;
150
151 atomic_t notify_count;
152
153 uint8_t mac[6];
154 uint8_t media_status;
155 } rndis = {
156 .mac = { 0x00, 0x00, 0x5E, 0x00, 0x53, 0x01 },
157 .mtu = NET_ETH_MTU, /* Ethernet frame */
158 .media_status = RNDIS_OBJECT_ID_MEDIA_DISCONNECTED,
159 .state = UNINITIALIZED,
160 .skip_bytes = 0,
161 .speed = 0,
162 };
163
164 static uint8_t manufacturer[] = CONFIG_USB_DEVICE_MANUFACTURER;
165 static uint32_t drv_version = 1U;
166
167 /**
168 * Assumes MaxPacketsPerTransfer of 1 and 802.2 (ethernet) medium.
169 */
170 #define RNDIS_BUF_SIZE (NET_ETH_MAX_FRAME_SIZE + sizeof(struct rndis_payload_packet))
171
172 static uint8_t tx_buf[RNDIS_BUF_SIZE];
173
174 /**
175 * TODO: package reception can be optimized to avoid rx_buf usage.
176 */
177 static uint8_t rx_buf[RNDIS_BUF_SIZE];
178
179 static uint32_t object_id_supported[] = {
180 RNDIS_OBJECT_ID_GEN_SUPP_LIST,
181 RNDIS_OBJECT_ID_GEN_HW_STATUS,
182 RNDIS_OBJECT_ID_GEN_SUPP_MEDIA,
183 RNDIS_OBJECT_ID_GEN_IN_USE_MEDIA,
184
185 RNDIS_OBJECT_ID_GEN_MAX_FRAME_SIZE,
186 RNDIS_OBJECT_ID_GEN_LINK_SPEED,
187 RNDIS_OBJECT_ID_GEN_BLOCK_TX_SIZE,
188 RNDIS_OBJECT_ID_GEN_BLOCK_RX_SIZE,
189
190 RNDIS_OBJECT_ID_GEN_VENDOR_ID,
191 RNDIS_OBJECT_ID_GEN_VENDOR_DESC,
192 RNDIS_OBJECT_ID_GEN_VENDOR_DRV_VER,
193
194 RNDIS_OBJECT_ID_GEN_PKT_FILTER,
195 RNDIS_OBJECT_ID_GEN_MAX_TOTAL_SIZE,
196 RNDIS_OBJECT_ID_GEN_CONN_MEDIA_STATUS,
197 RNDIS_OBJECT_ID_GEN_PHYSICAL_MEDIUM,
198 #if defined(USE_RNDIS_STATISTICS)
199 /* Using RNDIS statistics puts heavy load on
200 * USB bus, disable it for now
201 */
202 RNDIS_OBJECT_ID_GEN_TRANSMIT_OK,
203 RNDIS_OBJECT_ID_GEN_RECEIVE_OK,
204 RNDIS_OBJECT_ID_GEN_TRANSMIT_ERROR,
205 RNDIS_OBJECT_ID_GEN_RECEIVE_ERROR,
206 RNDIS_OBJECT_ID_GEN_RECEIVE_NO_BUF,
207 #endif /* USE_RNDIS_STATISTICS */
208 RNDIS_OBJECT_ID_802_3_PERMANENT_ADDRESS,
209 RNDIS_OBJECT_ID_802_3_CURR_ADDRESS,
210 RNDIS_OBJECT_ID_802_3_MCAST_LIST,
211 RNDIS_OBJECT_ID_802_3_MAX_LIST_SIZE,
212 RNDIS_OBJECT_ID_802_3_MAC_OPTIONS,
213 };
214
215 #define RNDIS_INT_EP_IDX 0
216 #define RNDIS_OUT_EP_IDX 1
217 #define RNDIS_IN_EP_IDX 2
218
219 static void rndis_bulk_out(uint8_t ep, enum usb_dc_ep_cb_status_code ep_status);
220
221 static struct usb_ep_cfg_data rndis_ep_data[] = {
222 {
223 .ep_cb = usb_transfer_ep_callback,
224 .ep_addr = RNDIS_INT_EP_ADDR
225 },
226 {
227 .ep_cb = rndis_bulk_out,
228 .ep_addr = RNDIS_OUT_EP_ADDR
229 },
230 {
231 .ep_cb = usb_transfer_ep_callback,
232 .ep_addr = RNDIS_IN_EP_ADDR
233 },
234 };
235
parse_rndis_header(const uint8_t * buffer,uint32_t buf_len)236 static int parse_rndis_header(const uint8_t *buffer, uint32_t buf_len)
237 {
238 struct rndis_payload_packet *hdr = (void *)buffer;
239 uint32_t len;
240
241 if (buf_len < sizeof(*hdr)) {
242 LOG_ERR("Too small packet len %u", buf_len);
243 return -EINVAL;
244 }
245
246 if (hdr->type != sys_cpu_to_le32(RNDIS_DATA_PACKET)) {
247 LOG_ERR("Wrong data packet type 0x%x",
248 sys_le32_to_cpu(hdr->type));
249 return -EINVAL;
250 }
251
252 len = sys_le32_to_cpu(hdr->len);
253 /*
254 * Calculate additional offset since payload_offset is calculated
255 * from the start of itself ;)
256 */
257 if (len < sys_le32_to_cpu(hdr->payload_offset) +
258 sys_le32_to_cpu(hdr->payload_len) +
259 offsetof(struct rndis_payload_packet, payload_offset)) {
260 LOG_ERR("Incorrect RNDIS packet");
261 return -EINVAL;
262 }
263
264 LOG_DBG("Parsing packet: len %u payload offset %u payload len %u",
265 len, sys_le32_to_cpu(hdr->payload_offset),
266 sys_le32_to_cpu(hdr->payload_len));
267
268 return len;
269 }
270
rndis_clean(void)271 void rndis_clean(void)
272 {
273 LOG_DBG("");
274
275 if (rndis.in_pkt) {
276 net_pkt_unref(rndis.in_pkt);
277
278 rndis.in_pkt = NULL;
279 rndis.in_pkt_len = 0;
280 }
281
282 rndis.skip_bytes = 0;
283 }
284
rndis_bulk_out(uint8_t ep,enum usb_dc_ep_cb_status_code ep_status)285 static void rndis_bulk_out(uint8_t ep, enum usb_dc_ep_cb_status_code ep_status)
286 {
287 uint32_t hdr_offset = 0U;
288 uint32_t len, read;
289
290 usb_read(ep, NULL, 0, &len);
291
292 LOG_DBG("EP 0x%x status %d len %u", ep, ep_status, len);
293
294 if (len > sizeof(rx_buf)) {
295 LOG_WRN("Trying to receive too much data, drop");
296 rndis_clean();
297 return;
298 }
299
300 usb_read(ep, rx_buf, len, &read);
301 if (len != read) {
302 LOG_ERR("Read %u instead of expected %u, skip the rest",
303 read, len);
304 rndis.skip_bytes = len - read;
305 return;
306 }
307
308 /* We already use frame keeping with len, warn here about
309 * receiving frame delimiter
310 */
311 if (len == 1U && !rx_buf[0]) {
312 LOG_DBG("Got frame delimiter, skip");
313 return;
314 }
315
316 /* Handle skip bytes */
317 if (rndis.skip_bytes) {
318 LOG_WRN("Skip %u bytes out of remaining %d bytes",
319 len, rndis.skip_bytes);
320
321 rndis.skip_bytes -= len;
322
323 if (rndis.skip_bytes < 0) {
324 LOG_ERR("Error skipping bytes");
325
326 rndis.skip_bytes = 0;
327 }
328
329 return;
330 }
331
332 /* Start new packet */
333 if (!rndis.in_pkt) {
334 struct net_pkt *pkt;
335
336 /* Append data only, skipping RNDIS header */
337 hdr_offset = sizeof(struct rndis_payload_packet);
338
339 rndis.in_pkt_len = parse_rndis_header(rx_buf, len);
340 if (rndis.in_pkt_len < 0) {
341 LOG_ERR("Error parsing RNDIS header");
342
343 rndis.rx_err++;
344 return;
345 }
346
347 pkt = net_pkt_rx_alloc_with_buffer(netusb_net_iface(),
348 rndis.in_pkt_len, AF_UNSPEC,
349 0, K_NO_WAIT);
350 if (!pkt) {
351 /* In case of low memory: skip the whole packet
352 * hoping to get buffers for later ones
353 */
354 rndis.skip_bytes = rndis.in_pkt_len - len;
355 rndis.rx_no_buf++;
356
357 LOG_ERR("Not enough pkt buffers, len %u, skip %u",
358 rndis.in_pkt_len, rndis.skip_bytes);
359
360 return;
361 }
362
363 rndis.in_pkt = pkt;
364 }
365
366 if (net_pkt_write(rndis.in_pkt,
367 rx_buf + hdr_offset, len - hdr_offset)) {
368 LOG_ERR("Error writing data to pkt: %p", rndis.in_pkt);
369 rndis_clean();
370 rndis.rx_err++;
371 return;
372 }
373
374 LOG_DBG("To assemble %d bytes, reading %u bytes",
375 rndis.in_pkt_len, len);
376
377 rndis.in_pkt_len -= len;
378 if (!rndis.in_pkt_len) {
379 LOG_DBG("Assembled full RNDIS packet");
380
381 if (VERBOSE_DEBUG) {
382 net_pkt_hexdump(rndis.in_pkt, ">");
383 }
384
385 /* Queue data to iface */
386 netusb_recv(rndis.in_pkt);
387
388 /* Start over for new packets */
389 rndis.in_pkt = NULL;
390 } else if (rndis.in_pkt_len < 0) {
391 LOG_ERR("Error assembling packet, drop and start over");
392 rndis_clean();
393 }
394 }
395
rndis_notify_cb(uint8_t ep,int size,void * priv)396 static void rndis_notify_cb(uint8_t ep, int size, void *priv)
397 {
398 LOG_DBG("ep %x size %u", ep, size);
399
400
401 atomic_dec(&rndis.notify_count);
402 }
403
rndis_queue_rsp(struct net_buf * rsp)404 static void rndis_queue_rsp(struct net_buf *rsp)
405 {
406 if (!k_fifo_is_empty(&rndis_tx_queue)) {
407 LOG_WRN("Transmit response queue is not empty");
408 }
409
410 LOG_DBG("Queued response pkt %p", rsp);
411
412 net_buf_put(&rndis_tx_queue, rsp);
413 }
414
415 /* Notify host about available data */
rndis_notify_rsp(void)416 static void rndis_notify_rsp(void)
417 {
418 static uint32_t buf[2] = {
419 sys_cpu_to_le32(0x01),
420 sys_cpu_to_le32(0x00)
421 };
422 int ret;
423
424 LOG_DBG("count %lu", atomic_get(&rndis.notify_count));
425
426 if (atomic_get(&rndis.notify_count)) {
427 LOG_WRN("Notification is already sent");
428 return;
429 }
430
431 atomic_inc(&rndis.notify_count);
432
433 ret = usb_transfer(rndis_ep_data[RNDIS_INT_EP_IDX].ep_addr,
434 (uint8_t *)buf, sizeof(buf),
435 USB_TRANS_WRITE | USB_TRANS_NO_ZLP,
436 rndis_notify_cb, NULL);
437 if (ret < 0) {
438 LOG_ERR("Transfer failure, ret %d", ret);
439 }
440 }
441
rndis_init_handle(uint8_t * data,uint32_t len)442 static int rndis_init_handle(uint8_t *data, uint32_t len)
443 {
444 struct rndis_init_cmd *cmd = (void *)data;
445 struct rndis_init_cmd_complete *rsp;
446 struct net_buf *buf;
447
448 LOG_DBG("req_id 0x%x", cmd->req_id);
449
450 buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
451 if (!buf) {
452 LOG_ERR("Cannot get free buffer");
453 return -ENOMEM;
454 }
455
456 rsp = net_buf_add(buf, sizeof(*rsp));
457 rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
458 rsp->type = sys_cpu_to_le32(RNDIS_CMD_INITIALIZE_COMPLETE);
459 rsp->len = sys_cpu_to_le32(sizeof(*rsp));
460 rsp->req_id = cmd->req_id;
461
462 rsp->major_ver = sys_cpu_to_le32(RNDIS_MAJOR_VERSION);
463 rsp->minor_ver = sys_cpu_to_le32(RNDIS_MINOR_VERSION);
464
465 rsp->flags = sys_cpu_to_le32(RNDIS_FLAG_CONNECTIONLESS);
466 rsp->medium = sys_cpu_to_le32(RNDIS_MEDIUM_WIRED_ETHERNET);
467 rsp->max_packets = sys_cpu_to_le32(1);
468 rsp->max_transfer_size = sys_cpu_to_le32(RNDIS_BUF_SIZE);
469
470 rsp->pkt_align_factor = sys_cpu_to_le32(0);
471 (void)memset(rsp->__reserved, 0, sizeof(rsp->__reserved));
472
473 rndis.state = INITIALIZED;
474
475 rndis_queue_rsp(buf);
476
477 /* Notify about ready reply */
478 rndis_notify_rsp();
479
480 return 0;
481 }
482
rndis_halt_handle(void)483 static int rndis_halt_handle(void)
484 {
485 LOG_DBG("");
486
487 rndis.state = UNINITIALIZED;
488
489 /* TODO: Stop networking */
490
491 return 0;
492 }
493
rndis_query_add_supp_list(struct net_buf * buf)494 static uint32_t rndis_query_add_supp_list(struct net_buf *buf)
495 {
496 for (int i = 0; i < ARRAY_SIZE(object_id_supported); i++) {
497 net_buf_add_le32(buf, object_id_supported[i]);
498 }
499
500 return sizeof(object_id_supported);
501 }
502
rndis_query_handle(uint8_t * data,uint32_t len)503 static int rndis_query_handle(uint8_t *data, uint32_t len)
504 {
505 struct rndis_query_cmd *cmd = (void *)data;
506 struct rndis_query_cmd_complete *rsp;
507 struct net_buf *buf;
508 uint32_t object_id, buf_len = 0U;
509
510 buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
511 if (!buf) {
512 LOG_ERR("Cannot get free buffer");
513 return -ENOMEM;
514 }
515
516 object_id = sys_le32_to_cpu(cmd->object_id);
517
518 LOG_DBG("req_id 0x%x Object ID 0x%x buf_len %u buf_offset %u",
519 sys_le32_to_cpu(cmd->req_id),
520 object_id,
521 sys_le32_to_cpu(cmd->buf_len),
522 sys_le32_to_cpu(cmd->buf_offset));
523
524 rsp = net_buf_add(buf, sizeof(*rsp));
525 rsp->type = sys_cpu_to_le32(RNDIS_CMD_QUERY_COMPLETE);
526 rsp->req_id = cmd->req_id;
527
528 /* offset is from the beginning of the req_id field */
529 rsp->buf_offset = sys_cpu_to_le32(16);
530
531 switch (object_id) {
532 case RNDIS_OBJECT_ID_GEN_SUPP_LIST:
533 LOG_DBG("RNDIS_OBJECT_ID_GEN_SUPP_LIST");
534 rndis_query_add_supp_list(buf);
535 break;
536 case RNDIS_OBJECT_ID_GEN_PHYSICAL_MEDIUM:
537 LOG_DBG("RNDIS_OBJECT_ID_GEN_PHYSICAL_MEDIUM");
538 net_buf_add_le32(buf, RNDIS_PHYSICAL_MEDIUM_TYPE_UNSPECIFIED);
539 break;
540 case RNDIS_OBJECT_ID_GEN_MAX_FRAME_SIZE:
541 LOG_DBG("RNDIS_OBJECT_ID_GEN_MAX_FRAME_SIZE");
542 net_buf_add_le32(buf, rndis.mtu);
543 break;
544 case RNDIS_OBJECT_ID_GEN_LINK_SPEED:
545 LOG_DBG("RNDIS_OBJECT_ID_GEN_LINK_SPEED");
546 if (rndis.media_status == RNDIS_OBJECT_ID_MEDIA_DISCONNECTED) {
547 net_buf_add_le32(buf, 0);
548 } else {
549 net_buf_add_le32(buf, rndis.speed);
550 }
551 break;
552 case RNDIS_OBJECT_ID_GEN_CONN_MEDIA_STATUS:
553 LOG_DBG("RNDIS_OBJECT_ID_GEN_CONN_MEDIA_STATUS");
554 net_buf_add_le32(buf, rndis.media_status);
555 break;
556 case RNDIS_OBJECT_ID_GEN_MAX_TOTAL_SIZE:
557 LOG_DBG("RNDIS_OBJECT_ID_GEN_MAX_TOTAL_SIZE");
558 net_buf_add_le32(buf, RNDIS_GEN_MAX_TOTAL_SIZE);
559 break;
560
561 /* Statistics stuff */
562 case RNDIS_OBJECT_ID_GEN_TRANSMIT_ERROR:
563 LOG_DBG("RNDIS_OBJECT_ID_GEN_TRANSMIT_ERROR: %u", rndis.tx_err);
564 net_buf_add_le32(buf, rndis.tx_err);
565 break;
566 case RNDIS_OBJECT_ID_GEN_RECEIVE_ERROR:
567 LOG_DBG("RNDIS_OBJECT_ID_GEN_RECEIVE_ERROR: %u", rndis.rx_err);
568 net_buf_add_le32(buf, rndis.rx_err);
569 break;
570 case RNDIS_OBJECT_ID_GEN_RECEIVE_NO_BUF:
571 LOG_DBG("RNDIS_OBJECT_ID_GEN_RECEIVE_NO_BUF: %u",
572 rndis.rx_no_buf);
573 net_buf_add_le32(buf, rndis.rx_no_buf);
574 break;
575
576 /* IEEE 802.3 */
577 case RNDIS_OBJECT_ID_802_3_PERMANENT_ADDRESS:
578 LOG_DBG("RNDIS_OBJECT_ID_802_3_PERMANENT_ADDRESS");
579 memcpy(net_buf_add(buf, sizeof(rndis.mac)), rndis.mac,
580 sizeof(rndis.mac));
581 break;
582 case RNDIS_OBJECT_ID_802_3_CURR_ADDRESS:
583 LOG_DBG("RNDIS_OBJECT_ID_802_3_CURR_ADDRESS");
584 memcpy(net_buf_add(buf, sizeof(rndis.mac)), rndis.mac,
585 sizeof(rndis.mac));
586 break;
587 case RNDIS_OBJECT_ID_802_3_MCAST_LIST:
588 LOG_DBG("RNDIS_OBJECT_ID_802_3_MCAST_LIST");
589 net_buf_add_le32(buf, 0xE0000000); /* 224.0.0.0 */
590 break;
591 case RNDIS_OBJECT_ID_802_3_MAX_LIST_SIZE:
592 LOG_DBG("RNDIS_OBJECT_ID_802_3_MAX_LIST_SIZE");
593 net_buf_add_le32(buf, 1); /* one address */
594 break;
595
596 /* Vendor information */
597 case RNDIS_OBJECT_ID_GEN_VENDOR_ID:
598 LOG_DBG("RNDIS_OBJECT_ID_GEN_VENDOR_ID");
599 net_buf_add_le32(buf, CONFIG_USB_DEVICE_VID);
600 break;
601 case RNDIS_OBJECT_ID_GEN_VENDOR_DESC:
602 LOG_DBG("RNDIS_OBJECT_ID_GEN_VENDOR_DESC");
603 memcpy(net_buf_add(buf, sizeof(manufacturer) - 1), manufacturer,
604 sizeof(manufacturer) - 1);
605 break;
606 case RNDIS_OBJECT_ID_GEN_VENDOR_DRV_VER:
607 LOG_DBG("RNDIS_OBJECT_ID_GEN_VENDOR_DRV_VER");
608 net_buf_add_le32(buf, drv_version);
609 break;
610 default:
611 LOG_WRN("Unhandled query for Object ID 0x%x", object_id);
612 break;
613 }
614
615 buf_len = buf->len - sizeof(*rsp);
616
617 if (buf_len) {
618 rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
619 } else {
620 rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_NOT_SUPP);
621 }
622
623 /* Can be zero if object_id not handled / found */
624 rsp->buf_len = sys_cpu_to_le32(buf_len);
625
626 rsp->len = sys_cpu_to_le32(buf_len + sizeof(*rsp));
627
628 LOG_DBG("buf_len %u rsp->len %u buf->len %u",
629 buf_len, rsp->len, buf->len);
630
631 rndis_queue_rsp(buf);
632
633 /* Notify about ready reply */
634 rndis_notify_rsp();
635
636 return 0;
637 }
638
rndis_set_handle(uint8_t * data,uint32_t len)639 static int rndis_set_handle(uint8_t *data, uint32_t len)
640 {
641 struct rndis_set_cmd *cmd = (void *)data;
642 struct rndis_set_cmd_complete *rsp;
643 struct net_buf *buf;
644 uint32_t object_id;
645 uint8_t *param;
646
647 if (len < sizeof(*cmd)) {
648 LOG_ERR("Packet is shorter then header");
649 return -EINVAL;
650 }
651
652 /* Parameter starts at offset buf_offset of the req_id field ;) */
653 param = (uint8_t *)&cmd->req_id + sys_le32_to_cpu(cmd->buf_offset);
654
655 if (len - ((uintptr_t)param - (uintptr_t)cmd) !=
656 sys_le32_to_cpu(cmd->buf_len)) {
657 LOG_ERR("Packet parsing error");
658 return -EINVAL;
659 }
660
661 buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
662 if (!buf) {
663 LOG_ERR("Cannot get free buffer");
664 return -ENOMEM;
665 }
666
667 object_id = sys_le32_to_cpu(cmd->object_id);
668
669 LOG_DBG("req_id 0x%x Object ID 0x%x buf_len %u buf_offset %u",
670 sys_le32_to_cpu(cmd->req_id), object_id,
671 sys_le32_to_cpu(cmd->buf_len),
672 sys_le32_to_cpu(cmd->buf_offset));
673
674 rsp = net_buf_add(buf, sizeof(*rsp));
675 rsp->type = sys_cpu_to_le32(RNDIS_CMD_SET_COMPLETE);
676 rsp->len = sys_cpu_to_le32(sizeof(*rsp));
677 rsp->req_id = cmd->req_id; /* same endianness */
678
679 switch (object_id) {
680 case RNDIS_OBJECT_ID_GEN_PKT_FILTER:
681 if (sys_le32_to_cpu(cmd->buf_len) < sizeof(rndis.net_filter)) {
682 LOG_ERR("Packet is too small");
683 rsp->status = RNDIS_CMD_STATUS_INVALID_DATA;
684 break;
685 }
686
687 rndis.net_filter = sys_get_le32(param);
688 LOG_DBG("RNDIS_OBJECT_ID_GEN_PKT_FILTER 0x%x",
689 rndis.net_filter);
690 /* TODO: Start / Stop networking here */
691 rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
692 break;
693 case RNDIS_OBJECT_ID_802_3_MCAST_LIST:
694 LOG_DBG("RNDIS_OBJECT_ID_802_3_MCAST_LIST");
695 /* ignore for now */
696 rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
697 break;
698 default:
699 LOG_ERR("Unhandled object_id 0x%x", object_id);
700 rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_NOT_SUPP);
701 break;
702 }
703
704 rndis_queue_rsp(buf);
705
706 /* Notify about ready reply */
707 rndis_notify_rsp();
708
709 return 0;
710 }
711
rndis_reset_handle(uint8_t * data,uint32_t len)712 static int rndis_reset_handle(uint8_t *data, uint32_t len)
713 {
714 struct rndis_reset_cmd_complete *rsp;
715 struct net_buf *buf;
716
717 buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
718 if (!buf) {
719 LOG_ERR("Cannot get free buffer");
720 return -ENOMEM;
721 }
722
723 LOG_DBG("");
724
725 rsp = net_buf_add(buf, sizeof(*rsp));
726 rsp->type = sys_cpu_to_le32(RNDIS_CMD_RESET_COMPLETE);
727 rsp->len = sys_cpu_to_le32(sizeof(*rsp));
728 rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
729 rsp->addr_reset = sys_cpu_to_le32(1);
730
731 rndis_queue_rsp(buf);
732
733 /* Notify about ready reply */
734 rndis_notify_rsp();
735
736 return 0;
737 }
738
rndis_keepalive_handle(uint8_t * data,uint32_t len)739 static int rndis_keepalive_handle(uint8_t *data, uint32_t len)
740 {
741 struct rndis_keepalive_cmd *cmd = (void *)data;
742 struct rndis_keepalive_cmd_complete *rsp;
743 struct net_buf *buf;
744
745 buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
746 if (!buf) {
747 LOG_ERR("Cannot get free buffer");
748 return -ENOMEM;
749 }
750
751 LOG_DBG("");
752
753 rsp = net_buf_add(buf, sizeof(*rsp));
754 rsp->type = sys_cpu_to_le32(RNDIS_CMD_KEEPALIVE_COMPLETE);
755 rsp->len = sys_cpu_to_le32(sizeof(*rsp));
756 rsp->req_id = cmd->req_id; /* same order */
757 rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
758
759 rndis_queue_rsp(buf);
760
761 /* Notify about ready reply */
762 rndis_notify_rsp();
763
764 return 0;
765 }
766
queue_encapsulated_cmd(uint8_t * data,uint32_t len)767 static int queue_encapsulated_cmd(uint8_t *data, uint32_t len)
768 {
769 struct net_buf *buf;
770
771 buf = net_buf_alloc(&rndis_cmd_pool, K_NO_WAIT);
772 if (!buf) {
773 LOG_ERR("Cannot get free buffer");
774 return -ENOMEM;
775 }
776
777 memcpy(net_buf_add(buf, len), data, len);
778
779 net_buf_put(&rndis_cmd_queue, buf);
780
781 LOG_DBG("queued buf %p", buf);
782
783 return 0;
784 }
785
handle_encapsulated_cmd(uint8_t * data,uint32_t len)786 static int handle_encapsulated_cmd(uint8_t *data, uint32_t len)
787 {
788 struct tlv *msg = (void *)data;
789
790 if (VERBOSE_DEBUG) {
791 net_hexdump("CMD >", data, len);
792 }
793
794 if (len != msg->len) {
795 LOG_WRN("Total len is different then command len %u %u",
796 len, msg->len);
797 /* TODO: need actions? */
798 }
799
800 LOG_DBG("RNDIS type 0x%x len %u total len %u",
801 msg->type, msg->len, len);
802
803 switch (msg->type) {
804 case RNDIS_CMD_INITIALIZE:
805 return rndis_init_handle(data, len);
806 case RNDIS_CMD_HALT:
807 return rndis_halt_handle();
808 case RNDIS_CMD_QUERY:
809 return rndis_query_handle(data, len);
810 case RNDIS_CMD_SET:
811 return rndis_set_handle(data, len);
812 case RNDIS_CMD_RESET:
813 return rndis_reset_handle(data, len);
814 case RNDIS_CMD_KEEPALIVE:
815 return rndis_keepalive_handle(data, len);
816 default:
817 LOG_ERR("Message 0x%x unhandled", msg->type);
818 return -ENOTSUP;
819 }
820
821 return 0;
822 }
823
handle_encapsulated_rsp(uint8_t ** data,uint32_t * len)824 static int handle_encapsulated_rsp(uint8_t **data, uint32_t *len)
825 {
826 struct net_buf *buf;
827
828 LOG_DBG("");
829
830 buf = net_buf_get(&rndis_tx_queue, K_NO_WAIT);
831 if (!buf) {
832 LOG_ERR("Error getting response buffer");
833 *len = 0U;
834 return -ENODATA;
835 }
836
837 *len = buf->len;
838 if (*len > CONFIG_USB_REQUEST_BUFFER_SIZE) {
839 LOG_ERR("Response too long %u, truncating to %u", buf->len,
840 CONFIG_USB_REQUEST_BUFFER_SIZE);
841 *len = CONFIG_USB_REQUEST_BUFFER_SIZE;
842 }
843
844 if (VERBOSE_DEBUG) {
845 net_hexdump("RSP <", buf->data, buf->len);
846 }
847
848 memcpy(*data, buf->data, *len);
849
850 net_buf_unref(buf);
851
852 return 0;
853 }
854
rndis_class_handler(struct usb_setup_packet * setup,int32_t * len,uint8_t ** data)855 static int rndis_class_handler(struct usb_setup_packet *setup, int32_t *len,
856 uint8_t **data)
857 {
858 LOG_DBG("len %d req_type 0x%x req 0x%x enabled %u",
859 *len, setup->bmRequestType, setup->bRequest,
860 netusb_enabled());
861
862 if (!netusb_enabled()) {
863 LOG_ERR("interface disabled");
864 return -ENODEV;
865 }
866
867 if (usb_reqtype_is_to_device(setup)) {
868 if (setup->bRequest == CDC_SEND_ENC_CMD) {
869 /*
870 * Instead of handling here, queue
871 * handle_encapsulated_cmd(*data, *len);
872 */
873 return queue_encapsulated_cmd(*data, *len);
874 }
875 } else {
876 if (setup->bRequest == CDC_GET_ENC_RSP) {
877 return handle_encapsulated_rsp(data, len);
878 }
879 }
880
881 LOG_WRN("Unknown USB packet req 0x%x type 0x%x",
882 setup->bRequest, setup->bmRequestType);
883 return -ENOTSUP;
884 }
885
cmd_thread(void)886 static void cmd_thread(void)
887 {
888 LOG_INF("Command thread started");
889
890 while (true) {
891 struct net_buf *buf;
892
893 buf = net_buf_get(&rndis_cmd_queue, K_FOREVER);
894
895 LOG_DBG("got buf %p", buf);
896
897 handle_encapsulated_cmd(buf->data, buf->len);
898
899 net_buf_unref(buf);
900
901 k_yield();
902 }
903 }
904
905 /*
906 * RNDIS Send functions
907 */
908
rndis_hdr_add(uint8_t * buf,uint32_t len)909 static void rndis_hdr_add(uint8_t *buf, uint32_t len)
910 {
911 struct rndis_payload_packet *hdr = (void *)buf;
912 uint32_t offset = offsetof(struct rndis_payload_packet, payload_offset);
913
914 (void)memset(hdr, 0, sizeof(*hdr));
915
916 hdr->type = sys_cpu_to_le32(RNDIS_DATA_PACKET);
917 hdr->len = sys_cpu_to_le32(len + sizeof(*hdr));
918 hdr->payload_offset = sys_cpu_to_le32(sizeof(*hdr) - offset);
919 hdr->payload_len = sys_cpu_to_le32(len);
920
921 LOG_DBG("type %u len %u payload offset %u payload len %u",
922 hdr->type, hdr->len, hdr->payload_offset, hdr->payload_len);
923 }
924
rndis_send(struct net_pkt * pkt)925 static int rndis_send(struct net_pkt *pkt)
926 {
927 size_t len = net_pkt_get_len(pkt);
928 int ret;
929
930 LOG_DBG("send pkt %p len %u", pkt, len);
931
932 if (rndis.media_status == RNDIS_OBJECT_ID_MEDIA_DISCONNECTED) {
933 LOG_DBG("Media disconnected, drop pkt %p", pkt);
934 return -EPIPE;
935 }
936
937 if (VERBOSE_DEBUG) {
938 net_pkt_hexdump(pkt, "<");
939 }
940
941 if (len + sizeof(struct rndis_payload_packet) > sizeof(tx_buf)) {
942 LOG_WRN("Trying to send too large packet, drop");
943 return -ENOMEM;
944 }
945
946 rndis_hdr_add(tx_buf, len);
947
948 ret = net_pkt_read(pkt,
949 tx_buf + sizeof(struct rndis_payload_packet),
950 len);
951 if (ret < 0) {
952 return ret;
953 }
954
955 ret = usb_transfer_sync(rndis_ep_data[RNDIS_IN_EP_IDX].ep_addr, tx_buf,
956 len + sizeof(struct rndis_payload_packet),
957 USB_TRANS_WRITE);
958 if (ret != len + sizeof(struct rndis_payload_packet)) {
959 LOG_ERR("Transfer failure");
960 return ret;
961 }
962
963 return 0;
964 }
965
966 #if defined(CONFIG_USB_DEVICE_OS_DESC)
967 /* This string descriptor would be read the first time device is plugged in.
968 * It is Microsoft extension called an OS String Descriptor
969 */
970 #define MSOS_STRING_LENGTH 18
971 static struct string_desc {
972 uint8_t bLength;
973 uint8_t bDescriptorType;
974 uint8_t bString[MSOS_STRING_LENGTH - 4];
975 uint8_t bMS_VendorCode;
976 uint8_t bPad;
977 } __packed msosv1_string_descriptor = {
978 .bLength = MSOS_STRING_LENGTH,
979 .bDescriptorType = USB_DESC_STRING,
980 /* Signature MSFT100 */
981 .bString = {
982 'M', 0x00, 'S', 0x00, 'F', 0x00, 'T', 0x00,
983 '1', 0x00, '0', 0x00, '0', 0x00
984 },
985 .bMS_VendorCode = 0x03, /* Vendor Code, used for a control request */
986 .bPad = 0x00, /* Padding byte for VendorCode look as UTF16 */
987 };
988
989 static struct compat_id_desc {
990 /* MS OS 1.0 Header Section */
991 uint32_t dwLength;
992 uint16_t bcdVersion;
993 uint16_t wIndex;
994 uint8_t bCount;
995 uint8_t Reserved[7];
996 /* MS OS 1.0 Function Section */
997 struct compat_id_func {
998 uint8_t bFirstInterfaceNumber;
999 uint8_t Reserved1;
1000 uint8_t compatibleID[8];
1001 uint8_t subCompatibleID[8];
1002 uint8_t Reserved2[6];
1003 } __packed func[1];
1004 } __packed msosv1_compatid_descriptor = {
1005 .dwLength = sys_cpu_to_le32(40),
1006 .bcdVersion = sys_cpu_to_le16(0x0100),
1007 .wIndex = sys_cpu_to_le16(USB_OSDESC_EXTENDED_COMPAT_ID),
1008 .bCount = 0x01, /* One function section */
1009 .Reserved = {
1010 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
1011 },
1012
1013 .func = {
1014 {
1015 .bFirstInterfaceNumber = 0x00,
1016 .Reserved1 = 0x01,
1017 .compatibleID = {
1018 'R', 'N', 'D', 'I', 'S', 0x00, 0x00, 0x00
1019 },
1020 .subCompatibleID = {
1021 '5', '1', '6', '2', '0', '0', '1', 0x00
1022 },
1023 .Reserved2 = {
1024 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
1025 }
1026 },
1027 }
1028 };
1029
1030 static struct usb_os_descriptor os_desc = {
1031 .string = (uint8_t *)&msosv1_string_descriptor,
1032 .string_len = sizeof(msosv1_string_descriptor),
1033 .vendor_code = 0x03,
1034 .compat_id = (uint8_t *)&msosv1_compatid_descriptor,
1035 .compat_id_len = sizeof(msosv1_compatid_descriptor),
1036 };
1037 #endif /* CONFIG_USB_DEVICE_OS_DESC */
1038
rndis_init(void)1039 static int rndis_init(void)
1040 {
1041
1042 LOG_DBG("RNDIS initialization");
1043
1044 /* Transmit queue init */
1045 k_fifo_init(&rndis_tx_queue);
1046 /* Command queue init */
1047 k_fifo_init(&rndis_cmd_queue);
1048
1049 /* Register MS OS Descriptor */
1050 usb_register_os_desc(&os_desc);
1051
1052 k_thread_create(&cmd_thread_data, cmd_stack,
1053 K_KERNEL_STACK_SIZEOF(cmd_stack),
1054 (k_thread_entry_t)cmd_thread,
1055 NULL, NULL, NULL, K_PRIO_COOP(8), 0, K_NO_WAIT);
1056
1057 k_thread_name_set(&cmd_thread_data, "usb_rndis");
1058
1059 return 0;
1060 }
1061
rndis_connect_media(bool status)1062 static int rndis_connect_media(bool status)
1063 {
1064 if (status) {
1065 rndis.media_status = RNDIS_OBJECT_ID_MEDIA_CONNECTED;
1066 } else {
1067 rndis.media_status = RNDIS_OBJECT_ID_MEDIA_DISCONNECTED;
1068 }
1069
1070 return 0;
1071 }
1072
1073 static struct netusb_function rndis_function = {
1074 .connect_media = rndis_connect_media,
1075 .send_pkt = rndis_send,
1076 };
1077
rndis_status_cb(struct usb_cfg_data * cfg,enum usb_dc_status_code status,const uint8_t * param)1078 static void rndis_status_cb(struct usb_cfg_data *cfg,
1079 enum usb_dc_status_code status,
1080 const uint8_t *param)
1081 {
1082 ARG_UNUSED(cfg);
1083
1084 /* Check the USB status and do needed action if required */
1085 switch (status) {
1086 case USB_DC_CONFIGURED:
1087 LOG_DBG("USB device configured");
1088 netusb_enable(&rndis_function);
1089 break;
1090
1091 case USB_DC_DISCONNECTED:
1092 LOG_DBG("USB device disconnected");
1093 netusb_disable();
1094 break;
1095
1096 case USB_DC_ERROR:
1097 case USB_DC_RESET:
1098 case USB_DC_CONNECTED:
1099 case USB_DC_SUSPEND:
1100 case USB_DC_RESUME:
1101 case USB_DC_INTERFACE:
1102 LOG_DBG("USB unhandled state: %d", status);
1103 break;
1104
1105 case USB_DC_SOF:
1106 break;
1107
1108 case USB_DC_UNKNOWN:
1109 default:
1110 LOG_DBG("USB unknown state %d", status);
1111 break;
1112 }
1113 }
1114
netusb_interface_config(struct usb_desc_header * head,uint8_t bInterfaceNumber)1115 static void netusb_interface_config(struct usb_desc_header *head,
1116 uint8_t bInterfaceNumber)
1117 {
1118 ARG_UNUSED(head);
1119
1120 rndis_cfg.if0.bInterfaceNumber = bInterfaceNumber;
1121 rndis_cfg.if1.bInterfaceNumber = bInterfaceNumber + 1;
1122 rndis_cfg.iad.bFirstInterface = bInterfaceNumber;
1123 }
1124
1125 USBD_DEFINE_CFG_DATA(rndis_config) = {
1126 .usb_device_description = NULL,
1127 .interface_config = netusb_interface_config,
1128 .interface_descriptor = &rndis_cfg.if0,
1129 .cb_usb_status = rndis_status_cb,
1130 .interface = {
1131 .class_handler = rndis_class_handler,
1132 .custom_handler = NULL,
1133 .vendor_handler = NULL,
1134 },
1135 .num_endpoints = ARRAY_SIZE(rndis_ep_data),
1136 .endpoint = rndis_ep_data,
1137 };
1138
1139 /* Initialize this before eth_netusb device init */
1140 SYS_INIT(rndis_init, POST_KERNEL, 0);
1141
