Gitiles
Code Review Sign In
192.168.1.100 / T108 / e958203796650e3959a43708d67534bf36f4c83b / . / marvell / linux / drivers / usb / gadget / function / f_mbim.c
blob: 526d5869ee2fde6bdee86844776f1ddf91a6616d [file] [log] [blame]
b.liue9582032025-04-17 19:18:16 +0800[diff] [blame^]1/*
2 * f_mbim.c -- USB CDC Network (MBIM) link function driver
3 *
4 * Copyright (C) 2014 Marvell
5 * Contact: Yaniv Yizhak <yyaniv@marvell.com>
6 *
7 * The driver borrows from f_ecm.c which is:
8 *
9 * Copyright (C) 2003-2005,2008 David Brownell
10 * Copyright (C) 2008 Nokia Corporation
11 *
12 * This program is free software; you can redistribute it and/or modify
13 * it under the terms of the GNU General Public License as published by
14 * the Free Software Foundation; either version 2 of the License, or
15 * (at your option) any later version.
16 */
17#include <linux/kernel.h>
18#include <linux/device.h>
19#include <linux/etherdevice.h>
20#include <linux/crc32.h>
21#include <linux/usb/cdc.h>
22#include <linux/if_vlan.h>
23#include <linux/if_arp.h>
24#include <net/arp.h>
25#include <linux/spinlock.h>
26#include "u_ether.h"
27
28#ifdef CONFIG_ASR_TOE
29#define CONFIG_NCM_INPUT_AS_RNDIS 1
30#define CONFIG_MBIM_INPUT_AS_RNDIS 1
31#include <linux/toe.h>
32#if (defined CONFIG_NCM_INPUT_AS_RNDIS) || (defined CONFIG_MBIM_INPUT_AS_RNDIS)
33#include "rndis.h"
34#endif
35#endif
36
37
38static struct sk_buff *ncm_wrap_ntb(struct gether *port,
39 struct sk_buff *skb, struct aggr_ctx *aggr_ctx);
40static struct sk_buff *mbim_wrap_ntb(struct gether *port,
41 struct sk_buff *skb, struct aggr_ctx *aggr_ctx);
42#ifdef CONFIG_USB_G_NCM_MULT_PKT_SUPPORT
43static struct sk_buff *ncm_wrap_ntb_multipkt(struct gether *port,
44 struct sk_buff *skb, struct aggr_ctx *aggr_ctx);
45#endif
46#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
47static struct sk_buff *mbim_wrap_ntb_multipkt(struct gether *port,
48 struct sk_buff *skb, struct aggr_ctx *aggr_ctx);
49#endif
50static int ncm_unwrap_ntb(struct gether *port, struct sk_buff *skb,
51 struct sk_buff_head *list);
52static int mbim_unwrap_ntb(struct gether *port, struct sk_buff *skb,
53 struct sk_buff_head *list);
54#ifndef CONFIG_ASR_TOE
55static struct sk_buff *mbim_process_dgram(struct gether *port,
56 struct sk_buff *skb);
57#endif
58static void __maybe_unused mbim_net_open(struct gether *geth);
59static void __maybe_unused mbim_net_close(struct gether *geth);
60
61/*
62 * This function is a "Mobile Broadband Interface Model" (MBIM) link.
63 * MBIM is intended to be used with high-speed network attachments.
64 *
65 * Note that MBIM requires the use of "alternate settings" for its data
66 * interface. This means that the set_alt() method has real work to do,
67 * and also means that a get_alt() method is required.
68 */
69
70/* to trigger crc/non-crc NDP (NCM Datagram Pointer) signature */
71#define NCM_NDP_HDR_CRC_MASK 0x01000000
72#define NCM_NDP_HDR_CRC 0x01000000
73#define NCM_NDP_HDR_NOCRC 0x00000000
74
75/*
76 * Use wMaxPacketSize big enough to fit CDC_NOTIFY_SPEED_CHANGE in one
77 * packet, to simplify cancellation; and a big transfer interval, to
78 * waste less bandwidth.
79 */
80#define LOG2_STATUS_INTERVAL_MSEC 5 /* 1 << 5 == 32 msec */
81#define NCM_STATUS_BYTECOUNT 16 /* 8 byte header + data */
82#define MBIM_STATUS_BYTECOUNT (NCM_STATUS_BYTECOUNT)
83
84enum mbim_notify_state {
85 MBIM_NOTIFY_NONE, /* don't notify */
86 MBIM_NOTIFY_CONNECT, /* issue CONNECT next */
87 MBIM_NOTIFY_SPEED, /* issue SPEED_CHANGE next */
88};
89
90enum ncm_mbim_state {
91 STATE_NCM,
92 STATE_MBIM,
93};
94
95enum ncm_mbim_mode {
96 MODE_NCM,
97 MODE_MBIM,
98 MODE_NCM_MBIM,
99 MODE_UNKNOWN,
100};
101
102/* Control packet definition */
103#define MAX_CTRL_PKT_SIZE (4096)
104struct ctrl_pkt {
105 void *buf;
106 int len;
107 __le32 transaction_id;
108 struct list_head list;
109};
110
111struct f_mbim {
112 struct gether port;
113 char dev_addr[14];
114 char host_addr[ETH_ALEN];
115 struct usb_ep *notify;
116 struct usb_endpoint_descriptor *notify_desc;
117 struct usb_request *notify_req;
118 atomic_t notify_count;
119 u8 notify_state;
120 bool is_open;
121 atomic_t response_available;
122 atomic_t online;
123 atomic_t open_excl;
124 atomic_t ioctl_excl;
125 atomic_t read_excl;
126 atomic_t write_excl;
127 wait_queue_head_t read_wq;
128 wait_queue_head_t write_wq;
129 wait_queue_head_t open_wq;
130 u8 port_num;
131 u8 ctrl_id, data_id;
132 struct ndp_parser_opts *parser_opts;
133 bool is_crc;
134 spinlock_t lock;
135 struct list_head cpkt_req_q;
136 struct list_head cpkt_resp_q;
137 u32 ntb_input_size;
138 u16 ntb_max_datagrams;
139 atomic_t error;
140
141 enum ncm_mbim_mode mode;
142 enum ncm_mbim_state state;
143};
144
145/* Number of MBIM ports */
146#define NR_MBIM_PORTS 1
147static struct mbim_ports {
148 struct f_mbim *port;
149 unsigned port_num;
150} mbim_ports[NR_MBIM_PORTS];
151
152static struct usb_request *g_notify_req;
153
154static inline struct f_mbim *func_to_mbim(struct usb_function *f)
155{
156 return container_of(f, struct f_mbim, port.func);
157}
158
159/* peak (theoretical) bulk transfer rate in bits-per-second */
160static inline unsigned mbim_bitrate(struct usb_gadget *g)
161{
162 if (gadget_is_dualspeed(g) && g->speed == USB_SPEED_HIGH)
163 return (13 * 512 * 8 * 1000 * 8);
164 else
165 return (19 * 64 * 1 * 1000 * 8);
166}
167
168/*-------------------------------------------------------------------------*/
169
170/*
171 * skbs of size less than that will not be aligned
172 * to MBIM's dwNtbInMaxSize to save bus bandwidth
173 */
174#define MAX_TX_NONFIXED (512 * 3)
175
176/*
177 * Prevent memory waste here.
178 * NTB size 0x4000 is 16kB, but during skb allocation
179 * it will be reserved some additional memory in headroom,
180 * so final chunk to allocate is a little bit more than 16k.
181 * This leads to waste of memory because SLAB allocator
182 * will use 32kB buffers.
183 * So reduce maximum NTB size by 0x200 bytes that
184 * is quite enough for preventing usage of 32k buffers
185 * and performance doesn't have noticeable degradation
186 */
187
188#define MAX_IN_SKB_NUM (12) /* don't use too much headroom */
189#ifdef CONFIG_ASR_TOE
190#define NTB_IN_SIZE (16384)
191/* min value: ncm header(24+40*4) + 1514 + padding = 1700
192* max value: 2048 - 64(bm header) - sizeof(struct skb) = 1784
193*/
194#define NTB_OUT_SIZE (1728) /* 27 * 64 */
195#define MAX_DGRAMS_AGGREGATION (1)
196#else
197#ifndef CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS
198#define NTB_IN_SIZE (1600 * 10) /* 1500+14+64+alignment+extra 8bytes*/
199#define NTB_OUT_SIZE (2048)
200#define MAX_DGRAMS_AGGREGATION (10)
201#else
202#define NTB_IN_SIZE (16000) /* 1500+14+64+alignment+extra 8bytes*/
203#define NTB_OUT_SIZE (0x2000 - 256 - 2)
204#define MAX_DGRAMS_AGGREGATION (40)
205#endif
206#endif
207
208#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
209#ifdef CONFIG_USBNET_USE_SG
210#define MBIM_MAX_DGRAMS_AGGREGATION (8)
211#else
212#define MBIM_MAX_DGRAMS_AGGREGATION (10)
213#endif
214#endif
215
216#define NDP_IN_DIVISOR (0x4)
217
218
219#define FORMATS_SUPPORTED (USB_CDC_NCM_NTB16_SUPPORTED | USB_CDC_NCM_NTB32_SUPPORTED)
220
221/* NCM Transfer Block parameters (also used for GET_NTB_PARAMETERS request)*/
222static struct usb_cdc_ncm_ntb_parameters ntb_parameters = {
223 .wLength = sizeof ntb_parameters,
224 .bmNtbFormatsSupported = cpu_to_le16(FORMATS_SUPPORTED),
225 .dwNtbInMaxSize = cpu_to_le32(NTB_IN_SIZE),
226 .wNdpInDivisor = cpu_to_le16(NDP_IN_DIVISOR),
227 .wNdpInPayloadRemainder = cpu_to_le16(0),
228 .wNdpInAlignment = cpu_to_le16(4),
229
230 .dwNtbOutMaxSize = cpu_to_le32(NTB_OUT_SIZE),
231 .wNdpOutDivisor = cpu_to_le16(4),
232 .wNdpOutPayloadRemainder = cpu_to_le16(0),
233 .wNdpOutAlignment = cpu_to_le16(4),
234#ifdef CONFIG_ASR_TOE
235 .wNtbOutMaxDatagrams = 1,
236#else
237 .wNtbOutMaxDatagrams = 0, /* No limit */
238#endif
239};
240
241/*
242 * Here are options for the NCM Datagram Pointer table (NDP) parser.
243 * There are 2 different formats: NDP16 and NDP32 in the spec (ch. 3),
244 * in NDP16 offsets and sizes fields are 1 16bit word wide,
245 * in NDP32 -- 2 16bit words wide. Also signatures are different.
246 * To make the parser code the same, put the differences in the structure,
247 * and switch pointers to the structures when the format is changed.
248 */
249struct ndp_parser_opts {
250 u32 nth_sign;
251 u32 ndp_sign;
252 unsigned nth_size;
253 unsigned ndp_size;
254 unsigned ndplen_align;
255 /* sizes in u16 units */
256 unsigned dgram_item_len; /* index or length */
257 unsigned block_length;
258 unsigned fp_index;
259 unsigned reserved1;
260 unsigned reserved2;
261 unsigned next_fp_index;
262};
263
264#define INIT_NDP16_OPTS { \
265 .nth_sign = USB_CDC_NCM_NTH16_SIGN, \
266 .ndp_sign = USB_CDC_NCM_NDP16_NOCRC_SIGN, \
267 .nth_size = sizeof(struct usb_cdc_ncm_nth16), \
268 .ndp_size = sizeof(struct usb_cdc_ncm_ndp16), \
269 .ndplen_align = 4, \
270 .dgram_item_len = 1, \
271 .block_length = 1, \
272 .fp_index = 1, \
273 .reserved1 = 0, \
274 .reserved2 = 0, \
275 .next_fp_index = 1, \
276 }
277
278#define INIT_NDP32_OPTS { \
279 .nth_sign = USB_CDC_NCM_NTH32_SIGN, \
280 .ndp_sign = USB_CDC_NCM_NDP32_NOCRC_SIGN, \
281 .nth_size = sizeof(struct usb_cdc_ncm_nth32), \
282 .ndp_size = sizeof(struct usb_cdc_ncm_ndp32), \
283 .ndplen_align = 8, \
284 .dgram_item_len = 2, \
285 .block_length = 2, \
286 .fp_index = 2, \
287 .reserved1 = 1, \
288 .reserved2 = 2, \
289 .next_fp_index = 2, \
290 }
291
292static struct ndp_parser_opts ndp16_opts = INIT_NDP16_OPTS;
293static struct ndp_parser_opts ndp32_opts = INIT_NDP32_OPTS;
294
295/*---------------------------------------------------------------------------------*/
296/* USB descriptors */
297
298/* USB_DT_INTERFACE_ASSOCIATION: groups interfaces */
299static struct usb_interface_assoc_descriptor mbim_iad_desc = {
300 .bLength = sizeof mbim_iad_desc,
301 .bDescriptorType = USB_DT_INTERFACE_ASSOCIATION,
302
303 /* .bFirstInterface = DYNAMIC, */
304 .bInterfaceCount = 2, /* control + data */
305 .bFunctionClass = USB_CLASS_COMM,
306 .bFunctionSubClass = USB_CDC_SUBCLASS_MBIM,
307 .bFunctionProtocol = USB_CDC_PROTO_NONE,
308 /* .iFunction = DYNAMIC */
309};
310
311static struct usb_interface_assoc_descriptor ncm_iad_desc = {
312 .bLength = sizeof ncm_iad_desc,
313 .bDescriptorType = USB_DT_INTERFACE_ASSOCIATION,
314
315 /* .bFirstInterface = DYNAMIC, */
316 .bInterfaceCount = 2, /* control + data */
317 .bFunctionClass = USB_CLASS_COMM,
318 .bFunctionSubClass = USB_CDC_SUBCLASS_NCM,
319 .bFunctionProtocol = USB_CDC_PROTO_NONE,
320 /* .iFunction = DYNAMIC */
321};
322
323/* USB_DT_INTERFACE: Interface descriptor: CONTROL */
324static struct usb_interface_descriptor mbim_control_intf = {
325 .bLength = sizeof mbim_control_intf,
326 .bDescriptorType = USB_DT_INTERFACE,
327
328 /* .bInterfaceNumber = DYNAMIC */
329 .bNumEndpoints = 1,
330 .bInterfaceClass = USB_CLASS_COMM,
331 .bInterfaceSubClass = USB_CDC_SUBCLASS_MBIM,
332 .bInterfaceProtocol = USB_CDC_PROTO_NONE,
333 /* .iInterface = DYNAMIC */
334};
335
336static struct usb_interface_descriptor ncm_control_intf = {
337 .bLength = sizeof ncm_control_intf,
338 .bDescriptorType = USB_DT_INTERFACE,
339
340 /* .bInterfaceNumber = DYNAMIC */
341 .bNumEndpoints = 1,
342 .bInterfaceClass = USB_CLASS_COMM,
343 .bInterfaceSubClass = USB_CDC_SUBCLASS_NCM,
344 .bInterfaceProtocol = USB_CDC_PROTO_NONE,
345 /* .iInterface = DYNAMIC */
346};
347
348static struct usb_interface_descriptor ncm_mbim_control_intf = {
349 .bLength = sizeof mbim_control_intf,
350 .bDescriptorType = USB_DT_INTERFACE,
351
352 /* .bInterfaceNumber = DYNAMIC */
353 .bAlternateSetting = 1,
354 .bNumEndpoints = 1,
355 .bInterfaceClass = USB_CLASS_COMM,
356 .bInterfaceSubClass = USB_CDC_SUBCLASS_MBIM,
357 .bInterfaceProtocol = USB_CDC_PROTO_NONE,
358 /* .iInterface = DYNAMIC */
359};
360
361
362
363/* "Header Functional Descriptor" from CDC spec 5.2.3.1 */
364static struct usb_cdc_header_desc mbim_header_desc = {
365 .bLength = sizeof mbim_header_desc,
366 .bDescriptorType = USB_DT_CS_INTERFACE,
367 .bDescriptorSubType = USB_CDC_HEADER_TYPE,
368
369 .bcdCDC = cpu_to_le16(0x0110),
370};
371
372/* "Union Functional Descriptor" from CDC spec 5.2.3.8 */
373static struct usb_cdc_union_desc mbim_union_desc = {
374 .bLength = sizeof(mbim_union_desc),
375 .bDescriptorType = USB_DT_CS_INTERFACE,
376 .bDescriptorSubType = USB_CDC_UNION_TYPE,
377 /* .bMasterInterface0 = DYNAMIC */
378 /* .bSlaveInterface0 = DYNAMIC */
379};
380
381/* "Ethernet Networking Functional Descriptor" from CDC spec 5.2.3.16 */
382static struct usb_cdc_ether_desc ether_desc = {
383 .bLength = sizeof ether_desc,
384 .bDescriptorType = USB_DT_CS_INTERFACE,
385 .bDescriptorSubType = USB_CDC_ETHERNET_TYPE,
386
387 /* this descriptor actually adds value, surprise! */
388 /* .iMACAddress = DYNAMIC */
389 .bmEthernetStatistics = cpu_to_le32(0), /* no statistics */
390 .wMaxSegmentSize = cpu_to_le16(ETH_FRAME_LEN),
391 .wNumberMCFilters = cpu_to_le16(0),
392 .bNumberPowerFilters = 0,
393};
394
395/* "MBIM Control Model Functional Descriptor" */
396static struct usb_cdc_mbim_desc mbim_desc = {
397 .bLength = sizeof mbim_desc,
398 .bDescriptorType = USB_DT_CS_INTERFACE,
399 .bDescriptorSubType = USB_CDC_MBIM_TYPE,
400
401 .bcdMBIMVersion = cpu_to_le16(0x0100),
402 .wMaxControlMessage = cpu_to_le16(MAX_CTRL_PKT_SIZE),
403 .bNumberFilters = 0x10,
404 .bMaxFilterSize = 0x80,
405 .wMaxSegmentSize = cpu_to_le16(0xfe0),
406 .bmNetworkCapabilities = 0x20,
407};
408
409/* "MBIM Control Model Extended Functional Descriptor" */
410static struct usb_cdc_mbim_ext_desc mbim_ext_desc = {
411 .bLength = sizeof mbim_ext_desc,
412 .bDescriptorType = USB_DT_CS_INTERFACE,
413 .bDescriptorSubType = USB_CDC_MBIM_EXT_TYPE,
414
415 .bcdMBIMExtendedVersion = cpu_to_le16(0x0100),
416 .bMaxOutstandingCommandMessages = 1,
417 .wMTU = cpu_to_le16(1500),
418};
419
420#define NCAPS (USB_CDC_NCM_NCAP_ETH_FILTER)
421
422static struct usb_cdc_ncm_desc ncm_desc = {
423 .bLength = sizeof ncm_desc,
424 .bDescriptorType = USB_DT_CS_INTERFACE,
425 .bDescriptorSubType = USB_CDC_NCM_TYPE,
426
427 .bcdNcmVersion = cpu_to_le16(0x0100),
428 /* can process SetEthernetPacketFilter */
429 .bmNetworkCapabilities = NCAPS,
430};
431
432
433/* USB_DT_INTERFACE: Interface descriptor: DATA disable */
434static struct usb_interface_descriptor mbim_data_nop_intf = {
435 .bLength = sizeof mbim_data_nop_intf,
436 .bDescriptorType = USB_DT_INTERFACE,
437
438 /* .bInterfaceNumber = DYNAMIC */
439 .bAlternateSetting = 0,
440 .bNumEndpoints = 0,
441 .bInterfaceClass = USB_CLASS_CDC_DATA,
442 .bInterfaceSubClass = 0,
443 .bInterfaceProtocol = USB_CDC_MBIM_PROTO_NTB,
444 /* .iInterface = DYNAMIC */
445};
446
447static struct usb_interface_descriptor ncm_data_nop_intf = {
448 .bLength = sizeof ncm_data_nop_intf,
449 .bDescriptorType = USB_DT_INTERFACE,
450
451 .bInterfaceNumber = 1,
452 .bAlternateSetting = 0,
453 .bNumEndpoints = 0,
454 .bInterfaceClass = USB_CLASS_CDC_DATA,
455 .bInterfaceSubClass = 0,
456 .bInterfaceProtocol = USB_CDC_NCM_PROTO_NTB,
457 /* .iInterface = DYNAMIC */
458};
459
460/* USB_DT_INTERFACE: Interface descriptor: DATA enable */
461static struct usb_interface_descriptor mbim_data_intf = {
462 .bLength = sizeof mbim_data_intf,
463 .bDescriptorType = USB_DT_INTERFACE,
464
465 /* .bInterfaceNumber = DYNAMIC */
466 .bAlternateSetting = 1,
467 .bNumEndpoints = 2,
468 .bInterfaceClass = USB_CLASS_CDC_DATA,
469 .bInterfaceSubClass = 0,
470 .bInterfaceProtocol = USB_CDC_MBIM_PROTO_NTB,
471 /* .iInterface = DYNAMIC */
472};
473
474static struct usb_interface_descriptor ncm_data_intf = {
475 .bLength = sizeof ncm_data_intf,
476 .bDescriptorType = USB_DT_INTERFACE,
477
478 .bInterfaceNumber = 1,
479 .bAlternateSetting = 1,
480 .bNumEndpoints = 2,
481 .bInterfaceClass = USB_CLASS_CDC_DATA,
482 .bInterfaceSubClass = 0,
483 .bInterfaceProtocol = USB_CDC_NCM_PROTO_NTB,
484 /* .iInterface = DYNAMIC */
485};
486
487static struct usb_interface_descriptor ncm_mbim_data_intf = {
488 .bLength = sizeof mbim_data_intf,
489 .bDescriptorType = USB_DT_INTERFACE,
490
491 /* .bInterfaceNumber = DYNAMIC */
492 .bAlternateSetting = 2,
493 .bNumEndpoints = 2,
494 .bInterfaceClass = USB_CLASS_CDC_DATA,
495 .bInterfaceSubClass = 0,
496 .bInterfaceProtocol = USB_CDC_MBIM_PROTO_NTB,
497 /* .iInterface = DYNAMIC */
498};
499
500/* full speed endpoints support: */
501
502/* USB_DT_ENDPOINT: Endpoint descriptor: NOTIFY */
503static struct usb_endpoint_descriptor fs_mbim_notify_desc = {
504 .bLength = USB_DT_ENDPOINT_SIZE,
505 .bDescriptorType = USB_DT_ENDPOINT,
506
507 .bEndpointAddress = USB_DIR_IN,
508 .bmAttributes = USB_ENDPOINT_XFER_INT,
509 .wMaxPacketSize = cpu_to_le16(MBIM_STATUS_BYTECOUNT),
510 .bInterval = 1 << LOG2_STATUS_INTERVAL_MSEC,
511};
512
513/* USB_DT_ENDPOINT: Endpoint descriptor: IN */
514static struct usb_endpoint_descriptor fs_mbim_in_desc = {
515 .bLength = USB_DT_ENDPOINT_SIZE,
516 .bDescriptorType = USB_DT_ENDPOINT,
517
518 .bEndpointAddress = USB_DIR_IN,
519 .bmAttributes = USB_ENDPOINT_XFER_BULK,
520};
521
522/* USB_DT_ENDPOINT: Endpoint descriptor: OUT */
523static struct usb_endpoint_descriptor fs_mbim_out_desc = {
524 .bLength = USB_DT_ENDPOINT_SIZE,
525 .bDescriptorType = USB_DT_ENDPOINT,
526
527 .bEndpointAddress = USB_DIR_OUT,
528 .bmAttributes = USB_ENDPOINT_XFER_BULK,
529};
530
531/* List of all the full speed supported descriptors */
532static struct usb_descriptor_header *mbim_fs_function[] = {
533 (struct usb_descriptor_header *) &mbim_iad_desc,
534 /* CDC MBIM control descriptors */
535 (struct usb_descriptor_header *) &mbim_control_intf,
536 (struct usb_descriptor_header *) &mbim_header_desc,
537 (struct usb_descriptor_header *) &mbim_union_desc,
538 (struct usb_descriptor_header *) &ether_desc,
539 (struct usb_descriptor_header *) &mbim_desc,
540 (struct usb_descriptor_header *) &mbim_ext_desc,
541 (struct usb_descriptor_header *) &fs_mbim_notify_desc,
542 /* data interface, altsettings 0 and 1 */
543 (struct usb_descriptor_header *) &mbim_data_nop_intf,
544 (struct usb_descriptor_header *) &mbim_data_intf,
545 (struct usb_descriptor_header *) &fs_mbim_in_desc,
546 (struct usb_descriptor_header *) &fs_mbim_out_desc,
547 NULL,
548};
549
550static struct usb_descriptor_header *ncm_fs_function[] = {
551 (struct usb_descriptor_header *) &ncm_iad_desc,
552 /* CDC NCM control descriptors */
553 (struct usb_descriptor_header *) &ncm_control_intf,
554 (struct usb_descriptor_header *) &mbim_header_desc,
555 (struct usb_descriptor_header *) &mbim_union_desc,
556 (struct usb_descriptor_header *) &ether_desc,
557 (struct usb_descriptor_header *) &ncm_desc,
558 (struct usb_descriptor_header *) &fs_mbim_notify_desc,
559 /* data interface, altsettings 0 and 1 */
560 (struct usb_descriptor_header *) &ncm_data_nop_intf,
561 (struct usb_descriptor_header *) &ncm_data_intf,
562 (struct usb_descriptor_header *) &fs_mbim_in_desc,
563 (struct usb_descriptor_header *) &fs_mbim_out_desc,
564 NULL,
565};
566
567static struct usb_descriptor_header *ncm_mbim_fs_function[] = {
568 (struct usb_descriptor_header *) &ncm_iad_desc,
569 /* CDC NCM control descriptors alt 0 */
570 (struct usb_descriptor_header *) &ncm_control_intf,
571 (struct usb_descriptor_header *) &mbim_header_desc,
572 (struct usb_descriptor_header *) &mbim_union_desc,
573 (struct usb_descriptor_header *) &ether_desc,
574 (struct usb_descriptor_header *) &ncm_desc,
575 (struct usb_descriptor_header *) &fs_mbim_notify_desc,
576 /* CDC MBIM control descriptors alt 1 */
577 (struct usb_descriptor_header *) &ncm_mbim_control_intf,
578 (struct usb_descriptor_header *) &mbim_header_desc,
579 (struct usb_descriptor_header *) &mbim_union_desc,
580 (struct usb_descriptor_header *) &ether_desc,
581 (struct usb_descriptor_header *) &mbim_desc,
582 (struct usb_descriptor_header *) &mbim_ext_desc,
583 (struct usb_descriptor_header *) &fs_mbim_notify_desc,
584
585 /* data interface, altsettings 0, 1 and 2 */
586 (struct usb_descriptor_header *) &ncm_data_nop_intf,
587 (struct usb_descriptor_header *) &ncm_data_intf,
588 (struct usb_descriptor_header *) &fs_mbim_in_desc,
589 (struct usb_descriptor_header *) &fs_mbim_out_desc,
590 (struct usb_descriptor_header *) &ncm_mbim_data_intf,
591 (struct usb_descriptor_header *) &fs_mbim_in_desc,
592 (struct usb_descriptor_header *) &fs_mbim_out_desc,
593 NULL,
594};
595
596/* high speed endpoints support: */
597
598/* USB_DT_ENDPOINT: Endpoint descriptor: NOTIFY */
599static struct usb_endpoint_descriptor hs_mbim_notify_desc = {
600 .bLength = USB_DT_ENDPOINT_SIZE,
601 .bDescriptorType = USB_DT_ENDPOINT,
602
603 .bEndpointAddress = USB_DIR_IN,
604 .bmAttributes = USB_ENDPOINT_XFER_INT,
605 .wMaxPacketSize = cpu_to_le16(MBIM_STATUS_BYTECOUNT),
606 .bInterval = LOG2_STATUS_INTERVAL_MSEC + 4,
607};
608
609/* USB_DT_ENDPOINT: Endpoint descriptor: IN */
610static struct usb_endpoint_descriptor hs_mbim_in_desc = {
611 .bLength = USB_DT_ENDPOINT_SIZE,
612 .bDescriptorType = USB_DT_ENDPOINT,
613
614 .bEndpointAddress = USB_DIR_IN,
615 .bmAttributes = USB_ENDPOINT_XFER_BULK,
616 .wMaxPacketSize = cpu_to_le16(512),
617};
618
619/* USB_DT_ENDPOINT: Endpoint descriptor: OUT */
620static struct usb_endpoint_descriptor hs_mbim_out_desc = {
621 .bLength = USB_DT_ENDPOINT_SIZE,
622 .bDescriptorType = USB_DT_ENDPOINT,
623
624 .bEndpointAddress = USB_DIR_OUT,
625 .bmAttributes = USB_ENDPOINT_XFER_BULK,
626 .wMaxPacketSize = cpu_to_le16(512),
627};
628
629/* List of all the high speed supported descriptors */
630static struct usb_descriptor_header *mbim_hs_function[] = {
631 (struct usb_descriptor_header *) &mbim_iad_desc,
632 /* CDC MBIM control descriptors */
633 (struct usb_descriptor_header *) &mbim_control_intf,
634 (struct usb_descriptor_header *) &mbim_header_desc,
635 (struct usb_descriptor_header *) &mbim_union_desc,
636 (struct usb_descriptor_header *) &ether_desc,
637 (struct usb_descriptor_header *) &mbim_desc,
638 (struct usb_descriptor_header *) &mbim_ext_desc,
639 (struct usb_descriptor_header *) &hs_mbim_notify_desc,
640 /* data interface, altsettings 0 and 1 */
641 (struct usb_descriptor_header *) &mbim_data_nop_intf,
642 (struct usb_descriptor_header *) &mbim_data_intf,
643 (struct usb_descriptor_header *) &hs_mbim_in_desc,
644 (struct usb_descriptor_header *) &hs_mbim_out_desc,
645 NULL,
646};
647
648static struct usb_descriptor_header *ncm_hs_function[] = {
649 (struct usb_descriptor_header *) &ncm_iad_desc,
650 /* CDC NCM control descriptors */
651 (struct usb_descriptor_header *) &ncm_control_intf,
652 (struct usb_descriptor_header *) &mbim_header_desc,
653 (struct usb_descriptor_header *) &mbim_union_desc,
654 (struct usb_descriptor_header *) &ether_desc,
655 (struct usb_descriptor_header *) &ncm_desc,
656 (struct usb_descriptor_header *) &hs_mbim_notify_desc,
657 /* data interface, altsettings 0 and 1 */
658 (struct usb_descriptor_header *) &ncm_data_nop_intf,
659 (struct usb_descriptor_header *) &ncm_data_intf,
660 (struct usb_descriptor_header *) &hs_mbim_in_desc,
661 (struct usb_descriptor_header *) &hs_mbim_out_desc,
662 NULL,
663};
664
665static struct usb_descriptor_header *ncm_mbim_hs_function[] = {
666 (struct usb_descriptor_header *) &ncm_iad_desc,
667 /* CDC NCM control descriptors */
668 (struct usb_descriptor_header *) &ncm_control_intf,
669 (struct usb_descriptor_header *) &mbim_header_desc,
670 (struct usb_descriptor_header *) &mbim_union_desc,
671 (struct usb_descriptor_header *) &ether_desc,
672 (struct usb_descriptor_header *) &ncm_desc,
673 (struct usb_descriptor_header *) &hs_mbim_notify_desc,
674
675 (struct usb_descriptor_header *) &ncm_mbim_control_intf,
676 (struct usb_descriptor_header *) &mbim_header_desc,
677 (struct usb_descriptor_header *) &mbim_union_desc,
678 (struct usb_descriptor_header *) &ether_desc,
679 (struct usb_descriptor_header *) &mbim_desc,
680 (struct usb_descriptor_header *) &mbim_ext_desc,
681 (struct usb_descriptor_header *) &hs_mbim_notify_desc,
682 /* data interface, altsettings 0 and 1 */
683 (struct usb_descriptor_header *) &ncm_data_nop_intf,
684 (struct usb_descriptor_header *) &ncm_data_intf,
685 (struct usb_descriptor_header *) &hs_mbim_in_desc,
686 (struct usb_descriptor_header *) &hs_mbim_out_desc,
687 (struct usb_descriptor_header *) &ncm_mbim_data_intf,
688 (struct usb_descriptor_header *) &hs_mbim_in_desc,
689 (struct usb_descriptor_header *) &hs_mbim_out_desc,
690 NULL,
691};
692
693/* super speed endpoints support: */
694
695/* USB_DT_ENDPOINT: Endpoint descriptor: NOTIFY */
696static struct usb_endpoint_descriptor ss_mbim_notify_desc = {
697 .bLength = USB_DT_ENDPOINT_SIZE,
698 .bDescriptorType = USB_DT_ENDPOINT,
699
700 .bEndpointAddress = USB_DIR_IN,
701 .bmAttributes = USB_ENDPOINT_XFER_INT,
702 .wMaxPacketSize = cpu_to_le16(MBIM_STATUS_BYTECOUNT),
703 .bInterval = LOG2_STATUS_INTERVAL_MSEC + 4,
704};
705
706static struct usb_ss_ep_comp_descriptor ss_mbim_intr_comp_desc = {
707 .bLength = sizeof ss_mbim_intr_comp_desc,
708 .bDescriptorType = USB_DT_SS_ENDPOINT_COMP,
709
710 /* the following 3 values can be tweaked if necessary */
711 /* .bMaxBurst = 0, */
712 /* .bmAttributes = 0, */
713 .wBytesPerInterval = cpu_to_le16(MBIM_STATUS_BYTECOUNT),
714};
715
716/* USB_DT_ENDPOINT: Endpoint descriptor: IN */
717static struct usb_endpoint_descriptor ss_mbim_in_desc = {
718 .bLength = USB_DT_ENDPOINT_SIZE,
719 .bDescriptorType = USB_DT_ENDPOINT,
720
721 .bEndpointAddress = USB_DIR_IN,
722 .bmAttributes = USB_ENDPOINT_XFER_BULK,
723 .wMaxPacketSize = cpu_to_le16(1024),
724};
725
726/* USB_DT_ENDPOINT: Endpoint descriptor: OUT */
727static struct usb_endpoint_descriptor ss_mbim_out_desc = {
728 .bLength = USB_DT_ENDPOINT_SIZE,
729 .bDescriptorType = USB_DT_ENDPOINT,
730
731 .bEndpointAddress = USB_DIR_OUT,
732 .bmAttributes = USB_ENDPOINT_XFER_BULK,
733 .wMaxPacketSize = cpu_to_le16(1024),
734};
735
736static struct usb_ss_ep_comp_descriptor ss_mbim_bulk_comp_desc = {
737 .bLength = sizeof ss_mbim_bulk_comp_desc,
738 .bDescriptorType = USB_DT_SS_ENDPOINT_COMP,
739
740 /* the following 2 values can be tweaked if necessary */
741#ifdef CONFIG_ASR_TOE
742 .bMaxBurst = 15,
743#endif
744 /* .bmAttributes = 0, */
745};
746
747/* List of all the supper speed supported descriptors */
748static struct usb_descriptor_header *mbim_ss_function[] = {
749 (struct usb_descriptor_header *) &mbim_iad_desc,
750 /* CDC MBIM control descriptors */
751 (struct usb_descriptor_header *) &mbim_control_intf,
752 (struct usb_descriptor_header *) &mbim_header_desc,
753 (struct usb_descriptor_header *) &mbim_union_desc,
754 (struct usb_descriptor_header *) &ether_desc,
755 (struct usb_descriptor_header *) &mbim_desc,
756 (struct usb_descriptor_header *) &mbim_ext_desc,
757 (struct usb_descriptor_header *) &ss_mbim_notify_desc,
758 (struct usb_descriptor_header *) &ss_mbim_intr_comp_desc,
759 /* data interface, altsettings 0 and 1 */
760 (struct usb_descriptor_header *) &mbim_data_nop_intf,
761 (struct usb_descriptor_header *) &mbim_data_intf,
762 (struct usb_descriptor_header *) &ss_mbim_in_desc,
763 (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
764 (struct usb_descriptor_header *) &ss_mbim_out_desc,
765 (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
766 NULL,
767};
768
769static struct usb_descriptor_header *ncm_ss_function[] = {
770 (struct usb_descriptor_header *) &ncm_iad_desc,
771 /* CDC NCM control descriptors */
772 (struct usb_descriptor_header *) &ncm_control_intf,
773 (struct usb_descriptor_header *) &mbim_header_desc,
774 (struct usb_descriptor_header *) &mbim_union_desc,
775 (struct usb_descriptor_header *) &ether_desc,
776 (struct usb_descriptor_header *) &ncm_desc,
777 (struct usb_descriptor_header *) &ss_mbim_notify_desc,
778 (struct usb_descriptor_header *) &ss_mbim_intr_comp_desc,
779 /* data interface, altsettings 0 and 1 */
780 (struct usb_descriptor_header *) &ncm_data_nop_intf,
781 (struct usb_descriptor_header *) &ncm_data_intf,
782 (struct usb_descriptor_header *) &ss_mbim_in_desc,
783 (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
784 (struct usb_descriptor_header *) &ss_mbim_out_desc,
785 (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
786 NULL,
787};
788
789static struct usb_descriptor_header *ncm_mbim_ss_function[] = {
790 (struct usb_descriptor_header *) &ncm_iad_desc,
791 /* CDC NCM control descriptors */
792 (struct usb_descriptor_header *) &ncm_control_intf,
793 (struct usb_descriptor_header *) &mbim_header_desc,
794 (struct usb_descriptor_header *) &mbim_union_desc,
795 (struct usb_descriptor_header *) &ether_desc,
796 (struct usb_descriptor_header *) &ncm_desc,
797 (struct usb_descriptor_header *) &ss_mbim_notify_desc,
798 (struct usb_descriptor_header *) &ss_mbim_intr_comp_desc,
799 (struct usb_descriptor_header *) &ncm_mbim_control_intf,
800 (struct usb_descriptor_header *) &mbim_header_desc,
801 (struct usb_descriptor_header *) &mbim_union_desc,
802 (struct usb_descriptor_header *) &ether_desc,
803 (struct usb_descriptor_header *) &mbim_desc,
804 (struct usb_descriptor_header *) &mbim_ext_desc,
805 (struct usb_descriptor_header *) &ss_mbim_notify_desc,
806 (struct usb_descriptor_header *) &ss_mbim_intr_comp_desc,
807 /* data interface, altsettings 0 and 1 */
808 (struct usb_descriptor_header *) &ncm_data_nop_intf,
809 (struct usb_descriptor_header *) &ncm_data_intf,
810 (struct usb_descriptor_header *) &ss_mbim_in_desc,
811 (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
812 (struct usb_descriptor_header *) &ss_mbim_out_desc,
813 (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
814 (struct usb_descriptor_header *) &ncm_mbim_data_intf,
815 (struct usb_descriptor_header *) &ss_mbim_in_desc,
816 (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
817 (struct usb_descriptor_header *) &ss_mbim_out_desc,
818 (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
819 NULL,
820};
821
822
823/*---------------------------------------------------------------------------------*/
824/* string descriptors: */
825
826#define STRING_CTRL_IDX 0
827#define STRING_MAC_IDX 1
828#define STRING_DATA_IDX 2
829#define STRING_IAD_IDX 3
830
831static struct usb_string mbim_string_defs[] = {
832 [STRING_CTRL_IDX].s = "MBIM Control",
833 [STRING_MAC_IDX].s = NULL /* DYNAMIC */,
834 [STRING_DATA_IDX].s = "MBIM Data",
835 [STRING_IAD_IDX].s = "CDC MBIM",
836 { } /* end of list */
837};
838
839static struct usb_gadget_strings mbim_string_table = {
840 .language = 0x0409, /* en-us */
841 .strings = mbim_string_defs,
842};
843
844static struct usb_gadget_strings mbim_string_table_2 = {
845 .language = 0,
846 .strings = mbim_string_defs,
847};
848
849static struct usb_gadget_strings *mbim_strings[] = {
850 &mbim_string_table,
851 &mbim_string_table_2,
852 NULL,
853};
854
855/*Microsoft defined the following OS Descriptor*/
856#define USB_VENDOR_OSVC_DEFAULT 0x04
857#define OS_STRING_ID 0xEE /*Microsoft defined*/
858#define MS_EXTENDED_COMPAT_ID 0x04 /*Microsoft defined*/
859#define MS_EXTENDED_PROPERTY 0x05 /*Microsoft defined*/
860
861/* Microsoft MBIM OS String */
862static u8 mbim_os_string[] = {
863 18, /* sizeof(mtp_os_string) */
864 USB_DT_STRING,
865 /* Signature field: "MSFT100" */
866 'M', 0, 'S', 0, 'F', 0, 'T', 0, '1', 0, '0', 0, '0', 0,
867 /* vendor code */
868 USB_VENDOR_OSVC_DEFAULT,
869 /* padding */
870 0
871};
872
873/* Microsoft Extended Configuration Descriptor Header Section */
874struct mbim_ext_config_desc_header {
875 __le32 dwLength;
876 __u16 bcdVersion;
877 __le16 wIndex;
878 __u8 bCount;
879 __u8 reserved[7];
880};
881
882/* Microsoft Extended Configuration Descriptor Function Section */
883struct mbim_ext_config_desc_function {
884 __u8 bFirstInterfaceNumber;
885 __u8 bInterfaceCount;
886 __u8 compatibleID[8];
887 __u8 subCompatibleID[8];
888 __u8 reserved[6];
889};
890
891/* MBIM Extended Configuration Descriptor */
892struct {
893 struct mbim_ext_config_desc_header header;
894 struct mbim_ext_config_desc_function function;
895} mbim_ext_config_desc = {
896 .header = {
897 .dwLength = __constant_cpu_to_le32(sizeof(mbim_ext_config_desc)),
898 .bcdVersion = __constant_cpu_to_le16(0x0100),
899 .wIndex = __constant_cpu_to_le16(4),
900 .bCount = __constant_cpu_to_le16(1),
901 },
902 .function = {
903 .bFirstInterfaceNumber = 0,
904 .bInterfaceCount = 2,
905 .compatibleID = { 'M', 'B', 'I', 'M' },
906 },
907};
908
909#define MBIM_MAX_RESP_Q (64)
910/*-----------------------------------------------------------------------*/
911
912static inline void put_mbim(__le16 **p, unsigned size, unsigned val)
913{
914 switch (size) {
915 case 1:
916 put_unaligned_le16((u16)val, *p);
917 break;
918 case 2:
919 put_unaligned_le32((u32)val, *p);
920 break;
921 default:
922 BUG();
923 }
924
925 *p += size;
926}
927
928static inline void put_mbim_no_inc(void *p, unsigned size, unsigned val)
929{
930 switch (size) {
931 case 1:
932 put_unaligned_le16((u16)val, p);
933 break;
934 case 2:
935 put_unaligned_le32((u32)val, p);
936 break;
937 default:
938 BUG();
939 }
940}
941
942static inline unsigned get_mbim(__le16 **p, unsigned size)
943{
944 unsigned tmp;
945
946 switch (size) {
947 case 1:
948 tmp = get_unaligned_le16(*p);
949 break;
950 case 2:
951 tmp = get_unaligned_le32(*p);
952 break;
953 default:
954 BUG();
955 }
956
957 *p += size;
958 return tmp;
959}
960
961/*-------------------------------------------------------------------------*/
962static inline int mbim_lock(atomic_t *excl)
963{
964 if (atomic_inc_return(excl) == 1) {
965 return 0;
966 } else {
967 atomic_dec(excl);
968 return -EBUSY;
969 }
970}
971
972static inline void mbim_unlock(atomic_t *excl)
973{
974 atomic_dec(excl);
975}
976
977static struct ctrl_pkt *mbim_alloc_ctrl_pkt(unsigned len, gfp_t flags)
978{
979 struct ctrl_pkt *pkt;
980
981 pkt = kzalloc(sizeof(struct ctrl_pkt), flags);
982 if (!pkt)
983 return NULL;
984
985 pkt->buf = kmalloc(len, flags);
986 if (!pkt->buf) {
987 kfree(pkt);
988 return NULL;
989 }
990 pkt->len = len;
991
992 return pkt;
993}
994
995static void mbim_free_ctrl_pkt(struct ctrl_pkt *pkt)
996{
997 if (pkt) {
998 kfree(pkt->buf);
999 kfree(pkt);
1000 }
1001}
1002
1003static struct usb_request *mbim_alloc_req(struct usb_ep *ep, int buffer_size)
1004{
1005 struct usb_request *req = usb_ep_alloc_request(ep, GFP_KERNEL);
1006 if (!req)
1007 return NULL;
1008
1009 req->buf = kmalloc(buffer_size, GFP_KERNEL);
1010 if (!req->buf) {
1011 usb_ep_free_request(ep, req);
1012 return NULL;
1013 }
1014 req->length = buffer_size;
1015 return req;
1016}
1017
1018void fmbim_free_req(struct usb_ep *ep, struct usb_request *req)
1019{
1020 if (req) {
1021 kfree(req->buf);
1022 usb_ep_free_request(ep, req);
1023 }
1024}
1025
1026static void fmbim_ctrl_response_available_with_lock(struct f_mbim *dev, char need_lock)
1027{
1028 struct usb_request *req = dev->notify_req;
1029 struct usb_cdc_notification *event = NULL;
1030 unsigned long flags;
1031 int ret;
1032 struct ctrl_pkt *cpkt = NULL;
1033
1034 int notif_c = 0;
1035
1036 if (need_lock)
1037 spin_lock_irqsave(&dev->lock, flags);
1038
1039 if (!atomic_read(&dev->online)) {
1040 pr_info("dev:%p is not online\n", dev);
1041 if (need_lock)
1042 spin_unlock_irqrestore(&dev->lock, flags);
1043 return;
1044 }
1045
1046 if (!req) {
1047 /* Hold up to 64 responses in the circular queue */
1048 atomic_inc(&dev->response_available);
1049 pr_info_ratelimited("notify_req is NULL, try to queue the response, increment response available to %d\n",
1050 atomic_read(&dev->response_available));
1051 if (atomic_read(&dev->response_available) > MBIM_MAX_RESP_Q) {
1052 cpkt = list_first_entry(&dev->cpkt_resp_q, struct ctrl_pkt, list);
1053 BUG_ON(!cpkt);
1054 list_del(&cpkt->list);
1055 pr_info("cpkt=%p, transaction id=%d, removed from response queue head (too much responses)"
1056 , cpkt, cpkt->transaction_id);
1057 mbim_free_ctrl_pkt(cpkt);
1058 atomic_dec(&dev->response_available);
1059 }
1060 if (need_lock)
1061 spin_unlock_irqrestore(&dev->lock, flags);
1062 return;
1063 }
1064
1065 if (!req->buf) {
1066 pr_info("dev:%p req->buf is NULL\n", dev);
1067 if (need_lock)
1068 spin_unlock_irqrestore(&dev->lock, flags);
1069 return;
1070 }
1071
1072 notif_c = atomic_inc_return(&dev->notify_count);
1073 pr_info_ratelimited("atomic_inc_return[notif_c] = %d\n", notif_c);
1074
1075 event = req->buf;
1076 event->bmRequestType = USB_DIR_IN | USB_TYPE_CLASS
1077 | USB_RECIP_INTERFACE;
1078 event->bNotificationType = USB_CDC_NOTIFY_RESPONSE_AVAILABLE;
1079 event->wValue = cpu_to_le16(0);
1080 event->wIndex = cpu_to_le16(dev->ctrl_id);
1081 event->wLength = cpu_to_le16(0);
1082 dev->notify_req = NULL;
1083
1084 if (need_lock)
1085 spin_unlock_irqrestore(&dev->lock, flags);
1086
1087 pr_debug("Call usb_ep_queue\n");
1088
1089 ret = usb_ep_queue(dev->notify, req, GFP_ATOMIC);
1090 if (ret) {
1091 dev->notify_req = req;
1092 atomic_dec(&dev->notify_count);
1093 pr_err("ep enqueue error %d\n", ret);
1094 }
1095}
1096static void fmbim_ctrl_response_available(struct f_mbim *dev)
1097{
1098 /* Tell fmbim_ctrl_response_available_ext to use lock */
1099 fmbim_ctrl_response_available_with_lock(dev,1);
1100}
1101
1102static int
1103fmbim_send_cpkt_response(struct f_mbim *gr, struct ctrl_pkt *cpkt)
1104{
1105 struct f_mbim *dev = gr;
1106 unsigned long flags;
1107
1108 if (!gr || !cpkt) {
1109 pr_err("Invalid cpkt, dev:%p cpkt:%p\n",
1110 gr, cpkt);
1111 return -ENODEV;
1112 }
1113
1114 if (!atomic_read(&dev->online)) {
1115 pr_info("dev:%p is not connected\n", dev);
1116 mbim_free_ctrl_pkt(cpkt);
1117 return 0;
1118 }
1119
1120 pr_debug("Add packet to cpkt_resp_q trans_id=%d\n", cpkt->transaction_id);
1121 spin_lock_irqsave(&dev->lock, flags);
1122 list_add_tail(&cpkt->list, &dev->cpkt_resp_q);
1123 spin_unlock_irqrestore(&dev->lock, flags);
1124
1125 fmbim_ctrl_response_available(dev);
1126
1127 return 0;
1128}
1129
1130static void mbim_reset_values(struct f_mbim *mbim)
1131{
1132 mbim->parser_opts = &ndp16_opts;
1133 mbim->is_crc = false;
1134 mbim->port.cdc_filter = DEFAULT_FILTER;
1135 mbim->ntb_input_size = le32_to_cpu(ntb_parameters.dwNtbInMaxSize);;
1136 mbim->port.header_len = USB_CDC_NCM_MIN_HEADER16_SIZE;
1137 atomic_set(&mbim->notify_count, 0);
1138 atomic_set(&mbim->response_available, 0);
1139 atomic_set(&mbim->online, 0);
1140 mbim->port.fixed_out_len = le32_to_cpu(ntb_parameters.dwNtbOutMaxSize);
1141 mbim->port.fixed_in_len = le32_to_cpu(ntb_parameters.dwNtbInMaxSize);
1142}
1143
1144static void mbim_reset_function_queue(struct f_mbim *dev)
1145{
1146 struct ctrl_pkt *cpkt = NULL;
1147
1148 spin_lock(&dev->lock);
1149
1150 cpkt = mbim_alloc_ctrl_pkt(0, GFP_ATOMIC);
1151 if (!cpkt) {
1152 pr_err("Unable to allocate reset function pkt\n");
1153 spin_unlock(&dev->lock);
1154 return;
1155 }
1156
1157 list_add_tail(&cpkt->list, &dev->cpkt_req_q);
1158 spin_unlock(&dev->lock);
1159
1160 wake_up(&dev->read_wq);
1161}
1162
1163static void fmbim_reset_cmd_complete(struct usb_ep *ep, struct usb_request *req)
1164{
1165 struct f_mbim *dev = req->context;
1166
1167 mbim_reset_function_queue(dev);
1168}
1169
1170static void mbim_clear_queues(struct f_mbim *mbim)
1171{
1172 struct ctrl_pkt *cpkt = NULL;
1173 struct list_head *act, *tmp;
1174
1175 spin_lock(&mbim->lock);
1176 list_for_each_safe(act, tmp, &mbim->cpkt_req_q) {
1177 cpkt = list_entry(act, struct ctrl_pkt, list);
1178 list_del(&cpkt->list);
1179 mbim_free_ctrl_pkt(cpkt);
1180 }
1181 list_for_each_safe(act, tmp, &mbim->cpkt_resp_q) {
1182 cpkt = list_entry(act, struct ctrl_pkt, list);
1183 list_del(&cpkt->list);
1184 mbim_free_ctrl_pkt(cpkt);
1185 }
1186 spin_unlock(&mbim->lock);
1187}
1188/*
1189 * Context: mbim->lock held
1190 */
1191static void mbim_do_notify(struct f_mbim *mbim)
1192{
1193 struct usb_request *req = mbim->notify_req;
1194 struct usb_cdc_notification *event;
1195 struct usb_composite_dev *cdev = mbim->port.func.config->cdev;
1196 struct usb_cdc_speed_change *speed_data;
1197 int status;
1198
1199 pr_info("state = %d\n", mbim->notify_state);
1200
1201 if (unlikely(!req)) {
1202 pr_err("No request available\n");
1203 return;
1204 }
1205
1206 event = req->buf;
1207 switch (mbim->notify_state) {
1208 case MBIM_NOTIFY_NONE:
1209 return;
1210
1211 case MBIM_NOTIFY_CONNECT:
1212 event->bNotificationType = USB_CDC_NOTIFY_NETWORK_CONNECTION;
1213 if (mbim->is_open)
1214 event->wValue = cpu_to_le16(1);
1215 else
1216 event->wValue = cpu_to_le16(0);
1217 event->wLength = 0;
1218 req->length = sizeof(struct usb_cdc_notification);
1219
1220 pr_info("notify connect %s\n", mbim->is_open ? "true" : "false");
1221 mbim->notify_state = MBIM_NOTIFY_NONE;
1222 break;
1223
1224 case MBIM_NOTIFY_SPEED:
1225 event->bNotificationType = USB_CDC_NOTIFY_SPEED_CHANGE;
1226 event->wValue = cpu_to_le16(0);
1227 event->wLength = cpu_to_le16(8);
1228
1229 /* SPEED_CHANGE data is up/down speeds in bits/sec */
1230 speed_data = (req->buf + sizeof(struct usb_cdc_notification));
1231 speed_data->DLBitRRate = cpu_to_le32(mbim_bitrate(cdev->gadget));
1232 speed_data->ULBitRate = speed_data->DLBitRRate;
1233
1234 req->length = (sizeof(struct usb_cdc_notification) + sizeof(struct usb_cdc_speed_change));
1235
1236 pr_info("notify speed %d bps\n", mbim_bitrate(cdev->gadget));
1237 mbim->notify_state = MBIM_NOTIFY_CONNECT;
1238 break;
1239 }
1240 event->bmRequestType = 0xA1;
1241 event->wIndex = cpu_to_le16(mbim->ctrl_id);
1242
1243 atomic_inc(&mbim->notify_count);
1244 pr_info("notif_c = %d\n", atomic_read(&mbim->notify_count));
1245 mbim->notify_req = NULL;
1246 /*
1247 * In double buffering if there is a space in FIFO,
1248 * completion callback can be called right after the call,
1249 * so unlocking
1250 */
1251 spin_unlock(&mbim->lock);
1252 status = usb_ep_queue(mbim->notify, req, GFP_ATOMIC);
1253 spin_lock(&mbim->lock);
1254 if (status < 0) {
1255 mbim->notify_req = req;
1256 atomic_dec(&mbim->notify_count);
1257 mbim->notify_state = MBIM_NOTIFY_NONE;
1258 pr_err("usb_ep_queue failed, err: %d\n", status);
1259 }
1260}
1261
1262/*
1263 * Context: mbim->lock held
1264 */
1265static void mbim_notify(struct f_mbim *mbim)
1266{
1267 /*
1268 * NOTE on most versions of Linux, host side cdc-ethernet
1269 * won't listen for notifications until its netdevice opens.
1270 * The first notification then sits in the FIFO for a long
1271 * time, and the second one is queued.
1272 *
1273 * If mbim_notify() is called before the second (CONNECT)
1274 * notification is sent, then it will reset to send the SPEED
1275 * notificaion again (and again, and again), but it's not a problem
1276 */
1277 if (mbim->notify_state == MBIM_NOTIFY_NONE) {
1278 mbim->notify_state = MBIM_NOTIFY_SPEED;
1279 mbim_do_notify(mbim);
1280 } else {
1281 pr_info("skipped, notify in progress (state=%d)\n",
1282 mbim->notify_state);
1283 }
1284}
1285
1286static void mbim_notify_complete(struct usb_ep *ep, struct usb_request *req)
1287{
1288 struct f_mbim *mbim = req->context;
1289 struct usb_cdc_notification *event = req->buf;
1290 int notif_c;
1291
1292 spin_lock(&mbim->lock);
1293 switch (req->status) {
1294 case 0:
1295 pr_info_ratelimited("Notification 0x%02x sent\n", event->bNotificationType);
1296 if (unlikely(!atomic_read(&mbim->online))) {
1297 pr_info("%s: dev is offline\n", __func__);
1298 pr_info("notif_c = %d\n", atomic_read(&mbim->notify_count));
1299 mbim->notify_state = MBIM_NOTIFY_NONE;
1300 /* atomic_set(&mbim->notify_count, 0); */
1301 mbim->notify_req = req;
1302 spin_unlock(&mbim->lock);
1303 return;
1304 }
1305
1306 notif_c = atomic_dec_return(&mbim->notify_count);
1307 if (unlikely(notif_c)) {
1308 pr_err("notification count != 0 (%d)\n", notif_c);
1309 BUG();
1310 }
1311
1312 if (event->bNotificationType == USB_CDC_NOTIFY_RESPONSE_AVAILABLE) {
1313 BUG_ON(mbim->notify_state != MBIM_NOTIFY_NONE);
1314 mbim->notify_req = req;
1315#ifndef SERIAL_RESPONSE
1316 if (atomic_read(&mbim->response_available)) {
1317 pr_debug("Response available=%d (was busy"
1318 "before), try to send it again.\n",
1319 atomic_read(&mbim->response_available));
1320 atomic_dec(&mbim->response_available);
1321 fmbim_ctrl_response_available_with_lock(mbim,0);
1322 }
1323#endif
1324 spin_unlock(&mbim->lock);
1325 return;
1326 } else if (event->bNotificationType == USB_CDC_NOTIFY_SPEED_CHANGE ||
1327 event->bNotificationType == USB_CDC_NOTIFY_NETWORK_CONNECTION) {
1328 pr_debug("Continue to mbim_do_notify()\n");
1329 break;
1330 } else {
1331 pr_err("Unknown notification!!!\n");
1332 BUG();
1333 }
1334 break;
1335 case -ECONNRESET:
1336 case -ESHUTDOWN:
1337 /* connection gone */
1338 pr_info("ESHUTDOWN/ECONNRESET, connection gone\n");
1339 mbim->notify_state = MBIM_NOTIFY_NONE;
1340 atomic_set(&mbim->online, 0);
1341 atomic_set(&mbim->notify_count, 0);
1342 atomic_set(&mbim->response_available, 0);
1343 spin_unlock(&mbim->lock);
1344 mbim_clear_queues(mbim);
1345 mbim_reset_function_queue(mbim);
1346 spin_lock(&mbim->lock);
1347 break;
1348 default:
1349 pr_err("Unknown event %02x --> %d\n",
1350 event->bNotificationType, req->status);
1351 break;
1352 }
1353
1354 mbim->notify_req = req;
1355 mbim_do_notify(mbim);
1356
1357 spin_unlock(&mbim->lock);
1358}
1359
1360static void mbim_ep0out_complete(struct usb_ep *ep, struct usb_request *req)
1361{
1362 /* now for SET_NTB_INPUT_SIZE only */
1363 unsigned in_size = 0;
1364 struct usb_function *f = req->context;
1365 struct f_mbim *mbim = func_to_mbim(f);
1366 struct mbim_ntb_input_size {
1367 u32 ntb_input_size;
1368 u16 ntb_max_datagrams;
1369 u16 reserved;
1370 } *ntb = NULL;
1371 req->context = NULL;
1372 if (req->status || req->actual != req->length) {
1373 pr_err("Bad control-OUT transfer\n");
1374 goto invalid;
1375 }
1376
1377 if (req->length == 4) {
1378 in_size = get_unaligned_le32(req->buf);
1379 if (in_size < USB_CDC_NCM_NTB_MIN_IN_SIZE ||
1380 in_size > mbim->ntb_input_size) {
1381 pr_err("Illegal INPUT SIZE (%d) from host\n", in_size);
1382 goto invalid;
1383 }
1384 } else if (req->length == 8) {
1385 ntb = (struct mbim_ntb_input_size *)req->buf;
1386 in_size = get_unaligned_le32(&(ntb->ntb_input_size));
1387 if (in_size < USB_CDC_NCM_NTB_MIN_IN_SIZE ||
1388 in_size > mbim->ntb_input_size) {
1389 pr_err("Illegal INPUT SIZE (%d) from host\n", in_size);
1390 goto invalid;
1391 }
1392 mbim->ntb_max_datagrams =
1393 get_unaligned_le16(&(ntb->ntb_max_datagrams));
1394 } else {
1395 pr_err("Illegal NTB length %d\n", in_size);
1396 goto invalid;
1397 }
1398
1399 pr_debug("Set NTB INPUT SIZE %d\n", in_size);
1400
1401 mbim->ntb_input_size = in_size;
1402 mbim->port.fixed_in_len = in_size;
1403 return;
1404
1405invalid:
1406 usb_ep_set_halt(ep);
1407
1408 pr_err("dev:%p Failed\n", mbim);
1409
1410 return;
1411}
1412
1413static void
1414fmbim_cmd_complete(struct usb_ep *ep, struct usb_request *req)
1415{
1416 struct f_mbim *dev = req->context;
1417 struct ctrl_pkt *cpkt = NULL;
1418 int len = req->actual;
1419 struct usb_cdc_mbim_ctrl_msg *ctrl_msg = req->buf;
1420
1421 if (!dev) {
1422 pr_err("mbim dev is null\n");
1423 return;
1424 }
1425
1426 if (req->status < 0) {
1427 pr_err("mbim command error %d\n", req->status);
1428 return;
1429 }
1430
1431 spin_lock(&dev->lock);
1432 cpkt = mbim_alloc_ctrl_pkt(len, GFP_ATOMIC);
1433 if (!cpkt) {
1434 pr_err("Unable to allocate ctrl pkt\n");
1435 spin_unlock(&dev->lock);
1436 return;
1437 }
1438
1439 pr_debug("Add to cpkt_req_q packet with len = %d\n", len);
1440 pr_debug("MessagType = %d, TransactionId = %d\n",
1441 le32_to_cpu(ctrl_msg->dwMessagType),
1442 le32_to_cpu(ctrl_msg->dwTransactionId));
1443 cpkt->transaction_id = le32_to_cpu(ctrl_msg->dwTransactionId);
1444 memcpy(cpkt->buf, req->buf, len);
1445 list_add_tail(&cpkt->list, &dev->cpkt_req_q);
1446 spin_unlock(&dev->lock);
1447
1448 /* wakeup read thread */
1449 pr_debug("Wake up read queue\n");
1450 wake_up(&dev->read_wq);
1451
1452 return;
1453}
1454
1455static int
1456mbim_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
1457{
1458 struct f_mbim *mbim = func_to_mbim(f);
1459 struct usb_composite_dev *cdev = f->config->cdev;
1460 struct usb_request *req = cdev->req;
1461 struct ctrl_pkt *cpkt = NULL;
1462 int value = -EOPNOTSUPP;
1463 u16 w_index = le16_to_cpu(ctrl->wIndex);
1464 u16 w_value = le16_to_cpu(ctrl->wValue);
1465 u16 w_length = le16_to_cpu(ctrl->wLength);
1466 int response_available = 0;
1467
1468 /*
1469 * composite driver infrastructure handles everything except
1470 * CDC class messages; interface activation uses set_alt().
1471 */
1472
1473 /*
1474 * For NCM+MBIM function we need to define and handle
1475 * Microsoft OS specific strings/requests. This allows Win8
1476 * to select correct driver (MBIM) for NCM+MBIM fallback mode
1477 */
1478 if (ctrl->bRequestType ==
1479 (USB_DIR_IN | USB_TYPE_STANDARD | USB_RECIP_DEVICE)
1480 && ctrl->bRequest == USB_REQ_GET_DESCRIPTOR
1481 && (w_value >> 8) == USB_DT_STRING
1482 && (w_value & 0xFF) == OS_STRING_ID) {
1483
1484 pr_debug("OS string request: %d index: %d value: %d length: %d\n",
1485 ctrl->bRequest, w_index, w_value, w_length);
1486
1487 if(mbim->mode == MODE_NCM_MBIM) {
1488 value = (w_length < sizeof(mbim_os_string)
1489 ? w_length : sizeof(mbim_os_string));
1490 memcpy(cdev->req->buf, mbim_os_string, value);
1491 }
1492
1493 /* return here since composite.c will send for us */
1494 return value;
1495 }
1496
1497 /* Handle MBIM OS descriptor */
1498 if ((ctrl->bRequestType & USB_TYPE_MASK) == USB_TYPE_VENDOR
1499 && (ctrl->bRequest == USB_VENDOR_OSVC_DEFAULT)
1500 && (ctrl->bRequestType & USB_DIR_IN)
1501 && (w_index == MS_EXTENDED_COMPAT_ID
1502 || w_index == MS_EXTENDED_PROPERTY)) {
1503
1504 pr_debug("vendor request: %d index: %d value: %d length: %d\n",
1505 ctrl->bRequest, w_index, w_value, w_length);
1506
1507 if (mbim->mode == MODE_NCM_MBIM) {
1508 value = (w_length < sizeof(mbim_ext_config_desc) ?
1509 w_length : sizeof(mbim_ext_config_desc));
1510 memcpy(cdev->req->buf, &mbim_ext_config_desc, value);
1511 }
1512
1513 return value;
1514 }
1515
1516 switch ((ctrl->bRequestType << 8) | ctrl->bRequest) {
1517 case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1518 | USB_CDC_SET_ETHERNET_PACKET_FILTER:
1519 /*
1520 * see 6.2.30: no data, wIndex = interface,
1521 * wValue = packet filter bitmap
1522 */
1523 if (w_length != 0 || w_index != mbim->ctrl_id)
1524 goto invalid;
1525 pr_info_ratelimited("packet filter %02x\n", w_value);
1526 /*
1527 * REVISIT locking of cdc_filter. This assumes the UDC
1528 * driver won't have a concurrent packet TX irq running on
1529 * another CPU; or that if it does, this write is atomic...
1530 */
1531 mbim->port.cdc_filter = w_value;
1532 value = 0;
1533 break;
1534 /*
1535 * and optionally:
1536 * case USB_CDC_SEND_ENCAPSULATED_COMMAND:
1537 * case USB_CDC_GET_ENCAPSULATED_RESPONSE:
1538 * case USB_CDC_SET_ETHERNET_MULTICAST_FILTERS:
1539 * case USB_CDC_SET_ETHERNET_PM_PATTERN_FILTER:
1540 * case USB_CDC_GET_ETHERNET_PM_PATTERN_FILTER:
1541 * case USB_CDC_GET_ETHERNET_STATISTIC:
1542 */
1543 case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1544 | USB_CDC_RESET_FUNCTION:
1545
1546 pr_info_ratelimited("USB_CDC_RESET_FUNCTION\n");
1547 value = 0;
1548 req->complete = fmbim_reset_cmd_complete;
1549 req->context = mbim;
1550 break;
1551
1552 case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1553 | USB_CDC_SEND_ENCAPSULATED_COMMAND:
1554
1555 pr_info_ratelimited("USB_CDC_SEND_ENCAPSULATED_COMMAND\n");
1556
1557 value = w_length;
1558 req->complete = fmbim_cmd_complete;
1559 req->context = mbim;
1560 break;
1561
1562 case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1563 | USB_CDC_GET_ENCAPSULATED_RESPONSE:
1564
1565 pr_debug("USB_CDC_GET_ENCAPSULATED_RESPONSE\n");
1566
1567 if (w_value) {
1568 pr_err("w_value > 0: %d\n", w_value);
1569 break;
1570 }
1571
1572 spin_lock(&mbim->lock);
1573 if (list_empty(&mbim->cpkt_resp_q)) {
1574 pr_err("ctrl resp queue empty\n");
1575 spin_unlock(&mbim->lock);
1576 break;
1577 }
1578
1579 response_available = atomic_read(&mbim->response_available);
1580
1581 cpkt = list_first_entry(&mbim->cpkt_resp_q,
1582 struct ctrl_pkt, list);
1583 list_del(&cpkt->list);
1584 spin_unlock(&mbim->lock);
1585
1586 value = min_t(unsigned, w_length, cpkt->len);
1587 memcpy(req->buf, cpkt->buf, value);
1588 pr_debug("cpkt=%p, transaction id=%d, \n", cpkt, cpkt->transaction_id);
1589 mbim_free_ctrl_pkt(cpkt);
1590
1591 pr_info_ratelimited("copied encapsulated_response %d bytes\n",
1592 value);
1593 break;
1594
1595 case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1596 | USB_CDC_GET_NTB_PARAMETERS:
1597
1598 pr_info_ratelimited("USB_CDC_GET_NTB_PARAMETERS\n");
1599
1600 if (w_length == 0 || w_value != 0 || w_index != mbim->ctrl_id)
1601 goto invalid;
1602 value = w_length > sizeof ntb_parameters ?
1603 sizeof ntb_parameters : w_length;
1604 memcpy(req->buf, &ntb_parameters, value);
1605 break;
1606
1607 case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1608 | USB_CDC_GET_NTB_INPUT_SIZE:
1609
1610 pr_info_ratelimited("USB_CDC_GET_NTB_INPUT_SIZE\n");
1611
1612 if (w_length < 4 || w_value != 0 || w_index != mbim->ctrl_id)
1613 goto invalid;
1614 put_unaligned_le32(mbim->ntb_input_size, req->buf);
1615 value = 4;
1616 pr_info_ratelimited("Reply to host INPUT SIZE %d\n",
1617 mbim->ntb_input_size);
1618 break;
1619
1620 case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1621 | USB_CDC_SET_NTB_INPUT_SIZE:
1622
1623 pr_info_ratelimited("USB_CDC_SET_NTB_INPUT_SIZE\n");
1624
1625 if (w_length != 4 && w_length != 8) {
1626 pr_err("wrong NTB length %d\n", w_length);
1627 goto invalid;;
1628 }
1629
1630 if (w_value != 0 || w_index != mbim->ctrl_id)
1631 goto invalid;;
1632
1633 req->complete = mbim_ep0out_complete;
1634 req->length = w_length;
1635 req->context = f;
1636
1637 value = req->length;
1638 break;
1639
1640 case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1641 | USB_CDC_GET_NTB_FORMAT:
1642 {
1643 uint16_t format;
1644
1645 pr_info_ratelimited("USB_CDC_GET_NTB_FORMAT\n");
1646
1647 if (w_length < 2 || w_value != 0 || w_index != mbim->ctrl_id)
1648 goto invalid;
1649 format = (mbim->parser_opts == &ndp16_opts) ? 0x0000 : 0x0001;
1650 put_unaligned_le16(format, req->buf);
1651 value = 2;
1652 pr_info_ratelimited("NTB FORMAT: sending %d\n", format);
1653 break;
1654 }
1655
1656 case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1657 | USB_CDC_SET_NTB_FORMAT:
1658 {
1659 pr_info_ratelimited("USB_CDC_SET_NTB_FORMAT ");
1660
1661 if (w_length != 0 || w_index != mbim->ctrl_id)
1662 goto invalid;
1663 switch (w_value) {
1664 case 0x0000:
1665 mbim->parser_opts = &ndp16_opts;
1666 pr_info_ratelimited("MBIM16 selected\n");
1667 break;
1668 case 0x0001:
1669 mbim->parser_opts = &ndp32_opts;
1670 pr_info_ratelimited("MBIM32 selected\n");
1671 break;
1672 default:
1673 goto invalid;
1674 }
1675 value = 0;
1676 break;
1677 }
1678 case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1679 | USB_CDC_GET_CRC_MODE:
1680 {
1681 uint16_t is_crc;
1682
1683 if (w_length < 2 || w_value != 0 || w_index != mbim->ctrl_id)
1684 goto invalid;
1685 is_crc = mbim->is_crc ? 0x0001 : 0x0000;
1686 put_unaligned_le16(is_crc, req->buf);
1687 value = 2;
1688 pr_debug("Host asked CRC MODE, sending %d\n", is_crc);
1689 break;
1690 }
1691
1692 case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
1693 | USB_CDC_SET_CRC_MODE:
1694 {
1695 int ndp_hdr_crc = 0;
1696
1697 if (w_length != 0 || w_index != mbim->ctrl_id)
1698 goto invalid;
1699 switch (w_value) {
1700 case 0x0000:
1701 mbim->is_crc = false;
1702 ndp_hdr_crc = NCM_NDP_HDR_NOCRC;
1703 pr_debug("non-CRC mode selected\n");
1704 break;
1705 case 0x0001:
1706 mbim->is_crc = true;
1707 ndp_hdr_crc = NCM_NDP_HDR_CRC;
1708 pr_debug("CRC mode selected\n");
1709 break;
1710 default:
1711 goto invalid;
1712 }
1713 mbim->parser_opts->ndp_sign &= ~NCM_NDP_HDR_CRC_MASK;
1714 mbim->parser_opts->ndp_sign |= ndp_hdr_crc;
1715 value = 0;
1716 break;
1717 }
1718
1719 /* and disabled in mbim descriptor: */
1720 /* case USB_CDC_GET_NET_ADDRESS: */
1721 /* case USB_CDC_SET_NET_ADDRESS: */
1722 /* case USB_CDC_GET_MAX_DATAGRAM_SIZE: */
1723 /* case USB_CDC_SET_MAX_DATAGRAM_SIZE: */
1724
1725 default:
1726invalid:
1727 pr_err("invalid control req%02x.%02x v%04x i%04x l%d\n",
1728 ctrl->bRequestType, ctrl->bRequest,
1729 w_value, w_index, w_length);
1730 }
1731
1732 /* respond with data transfer or status phase? */
1733 if (value >= 0) {
1734 pr_debug("control request: %02x.%02x v%04x i%04x l%d\n",
1735 ctrl->bRequestType, ctrl->bRequest,
1736 w_value, w_index, w_length);
1737 req->zero = (value < w_length);
1738 req->length = value;
1739 value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
1740 if (value < 0) {
1741 pr_err("queueing req failed: %02x.%02x, err %d\n",
1742 ctrl->bRequestType,
1743 ctrl->bRequest, value);
1744 }
1745#ifdef SERIAL_RESPONSE
1746 if (response_available) {
1747 pr_debug("Response available=%d (was busy before), try to send it again.\n", response_available);
1748 atomic_dec(&mbim->response_available);
1749 fmbim_ctrl_response_available(mbim);
1750 }
1751#endif
1752 } else {
1753 pr_err("ctrl req err %d: %02x.%02x v%04x i%04x l%d\n",
1754 value, ctrl->bRequestType, ctrl->bRequest,
1755 w_value, w_index, w_length);
1756 }
1757
1758 /* device either stalls (value < 0) or reports success */
1759 return value;
1760}
1761
1762static int mbim_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
1763{
1764 int ret;
1765 struct f_mbim *mbim = func_to_mbim(f);
1766 struct usb_composite_dev *cdev = f->config->cdev;
1767
1768 /* Control interface has only altsetting 0 */
1769 if (intf == mbim->ctrl_id) {
1770
1771 pr_info("CONTROL_INTERFACE: alt = %d\n", alt);
1772
1773 if ((mbim->mode != MODE_NCM_MBIM && alt != 0) ||
1774 (mbim->mode == MODE_NCM_MBIM && alt > 1))
1775 goto fail;
1776
1777 if (mbim->notify->driver_data) {
1778 pr_info("reset mbim control %d\n", intf);
1779 usb_ep_disable(mbim->notify);
1780 }
1781
1782 if (mbim->mode == MODE_NCM || (mbim->mode == MODE_NCM_MBIM && alt == 0))
1783 mbim->state = STATE_NCM;
1784 else if (mbim->mode == MODE_MBIM || (mbim->mode == MODE_NCM_MBIM && alt == 1))
1785 mbim->state = STATE_MBIM;
1786 else
1787 BUG();
1788
1789 if (!(mbim->notify->desc)) {
1790 DBG(cdev, "init mbim ctrl %d\n", intf);
1791 if (config_ep_by_speed(cdev->gadget, f, mbim->notify))
1792 goto fail;
1793 }
1794
1795 usb_ep_enable(mbim->notify);
1796 mbim->notify->driver_data = mbim;
1797
1798 /* Data interface has up to 3 altsettings, 0-2 */
1799 } else if (intf == mbim->data_id) {
1800
1801 pr_info("DATA_INTERFACE: alt = %d\n", alt);
1802
1803 if ((mbim->mode != MODE_NCM_MBIM && alt > 1) ||
1804 (mbim->mode == MODE_NCM_MBIM && alt > 2))
1805 goto fail;
1806
1807 if (mbim->port.in_ep->driver_data) {
1808 pr_info("reset mbim\n");
1809 /* notify req is still queued on usb controller
1810 * suppose this should return the req to mbim->notify_req
1811 */
1812 if (unlikely(NULL == mbim->notify_req)) {
1813 ret = usb_ep_dequeue(mbim->notify, g_notify_req);
1814 pr_info("dequeue notify_req done ret: %d\n", ret);
1815 }
1816
1817 mbim_reset_values(mbim);
1818 mbim_clear_queues(mbim);
1819 mbim_reset_function_queue(mbim);
1820 gether_disconnect(&mbim->port);
1821 }
1822
1823 /*
1824 * CDC Network only sends data in non-default altsettings.
1825 * Changing altsettings resets filters, statistics, etc.
1826 */
1827 if (alt == 1 || alt == 2) {
1828 struct net_device *net;
1829 pr_info("Alt set %d, initialize ports\n", alt);
1830
1831 if (!mbim->port.in_ep->desc ||
1832 !mbim->port.out_ep->desc) {
1833 pr_info("init mbim\n");
1834 if (config_ep_by_speed(cdev->gadget, f,
1835 mbim->port.in_ep) ||
1836 config_ep_by_speed(cdev->gadget, f,
1837 mbim->port.out_ep)) {
1838 mbim->port.in_ep->desc = NULL;
1839 mbim->port.out_ep->desc = NULL;
1840 goto fail;
1841 }
1842 }
1843
1844 if (mbim->state == STATE_NCM) {
1845 mbim->port.open = mbim_net_open;
1846 mbim->port.close = mbim_net_close;
1847#ifdef CONFIG_USB_G_NCM_MULT_PKT_SUPPORT
1848 mbim->port.wrap = ncm_wrap_ntb_multipkt;
1849#else
1850 mbim->port.wrap = ncm_wrap_ntb;
1851#endif
1852 mbim->port.unwrap = ncm_unwrap_ntb;
1853 mbim->port.unwrap_fixup = NULL;
1854 } else { /* STATE_MBIM */
1855 /* Ruslan: originally was commented out in this driver*/
1856 mbim->port.open = NULL;
1857 mbim->port.close = NULL;
1858
1859#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
1860 mbim->port.wrap = mbim_wrap_ntb_multipkt;
1861#ifdef CONFIG_USBNET_USE_SG
1862 mbim->port.is_sg_mode = 1;
1863#endif
1864#else
1865 mbim->port.wrap = mbim_wrap_ntb;
1866#endif
1867 mbim->port.unwrap = mbim_unwrap_ntb;
1868#ifndef CONFIG_ASR_TOE
1869 mbim->port.unwrap_fixup = mbim_process_dgram;
1870#endif
1871 }
1872 /* TODO */
1873 /* Enable zlps by default for MBIM conformance;
1874 * override for musb_hdrc (avoids txdma ovhead)
1875 */
1876 mbim->port.is_zlp_ok = !(
1877 gadget_is_musbhdrc(cdev->gadget)
1878 );
1879 mbim->port.cdc_filter = DEFAULT_FILTER;
1880
1881 atomic_set(&mbim->online, 1);
1882
1883 spin_lock(&mbim->lock);
1884 if (mbim->is_open)
1885 mbim_notify(mbim);
1886 spin_unlock(&mbim->lock);
1887
1888#ifdef CONFIG_ASR_TOE
1889 if (mbim->state == STATE_NCM) {
1890 mbim->port.ueth_type = UETHER_NCM;
1891 } else { /* STATE_MBIM */
1892 mbim->port.ueth_type = UETHER_MBIM;
1893 }
1894#endif
1895 pr_info("activate mbim\n");
1896 net = gether_connect(&mbim->port);
1897 if (IS_ERR(net))
1898 return PTR_ERR(net);
1899 }
1900 } else
1901 goto fail;
1902
1903 /* wakeup file threads */
1904 wake_up(&mbim->open_wq);
1905 wake_up(&mbim->read_wq);
1906 wake_up(&mbim->write_wq);
1907
1908 return 0;
1909
1910fail:
1911 pr_err("ERROR: Illegal Interface\n");
1912 return -EINVAL;
1913}
1914
1915/*
1916 * Because the data interface supports multiple altsettings,
1917 * this MBIM function *MUST* implement a get_alt() method.
1918 */
1919static int mbim_get_alt(struct usb_function *f, unsigned intf)
1920{
1921 struct f_mbim *mbim = func_to_mbim(f);
1922
1923 if (intf == mbim->ctrl_id)
1924 return 0;
1925 return mbim->port.in_ep->driver_data ? 1 : 0;
1926}
1927
1928#define MBIM_SK_CB_SIGN (0x4D49424D) /* MBIM */
1929
1930struct mbim_cb {
1931 u32 sign;
1932 u16 tci;
1933};
1934
1935#define MBIM_SKB_CB(skb) ((struct mbim_cb *)((skb)->cb))
1936struct nth {
1937 __le32 dwSignature;
1938 __le16 wHeaderLength;
1939 __le16 wSequence;
1940 union {
1941 struct {
1942 __le16 wBlockLength;
1943 __le16 wFpIndex;
1944 } nth16 __attribute__ ((packed));
1945 struct {
1946 __le32 wBlockLength;
1947 __le32 wFpIndex;
1948 } nth32 __attribute__ ((packed));
1949 } nth_type;
1950} __attribute__ ((packed));
1951
1952struct ndp {
1953 __le32 dwSignature;
1954 __le16 wLength;
1955 union {
1956 struct {
1957 __le16 wNextFpIndex;
1958 } ndp16 __attribute__ ((packed));
1959 struct {
1960 __le16 wReserved6;
1961 __le32 wNextFpIndex;
1962 __le32 wReserved12;
1963 } ndp32 __attribute__ ((packed));
1964 } ndp_type;
1965} __attribute__ ((packed));
1966
1967
1968#define NCM_16BIT_SIGN (0x484d434e)
1969
1970#define NTH_FIELD(nth_header, field) \
1971 (mbim->parser_opts->nth_sign == NCM_16BIT_SIGN ? \
1972 (void *)&(nth_header->nth_type.nth16.field) : \
1973 (void *)&(nth_header->nth_type.nth32.field))
1974
1975#define NDP_FIELD(ndp_header, field) \
1976 (mbim->parser_opts->nth_sign == NCM_16BIT_SIGN ? \
1977 (void *)&(ndp_header->ndp_type.ndp16.field) : \
1978 (void *)&(ndp_header->ndp_type.ndp32.field))
1979
1980#define NTB_MAX_HEADER_SIZE(skb) (*(unsigned int *)skb->cb)
1981
1982#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
1983#ifndef CONFIG_USBNET_USE_SG
1984#define MBIM_NOT_USE_SG
1985#endif
1986#endif
1987
1988#ifndef CONFIG_ASR_TOE
1989#if (defined CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS) || (defined MBIM_NOT_USE_SG)
1990static struct sk_buff *add_ndp_header(struct gether *port,
1991 struct sk_buff *skb, struct aggr_ctx *aggr_ctx, __le32 sign)
1992{
1993 struct f_mbim *mbim = func_to_mbim(&port->func);
1994 struct sk_buff *first_skb, *prev_skb;
1995 int ncb_len;
1996 struct ndp *ndp_header;
1997 __le16 *tmp;
1998 __le16 offset = 0;
1999 int ndp_len = 0, ndp_align = ntb_parameters.wNdpInAlignment;
2000 int div = ntb_parameters.wNdpInDivisor;
2001 int rem = ntb_parameters.wNdpInPayloadRemainder;
2002 int pad = 0;
2003 int ndp_pad;
2004 struct ndp_parser_opts *opts = mbim->parser_opts;
2005
2006 ndp_len = opts->ndp_size;
2007 ndp_len += 2 * 2 * opts->dgram_item_len; /* Datagram entry */
2008 ndp_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
2009
2010 first_skb = skb_peek(&aggr_ctx->skb_list);
2011 prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
2012
2013 BUG_ON(skb_queue_empty(&aggr_ctx->skb_list));
2014 BUG_ON(!first_skb);
2015 BUG_ON(!prev_skb);
2016 BUG_ON(!aggr_ctx->total_size);
2017
2018 ndp_header = (void *)skb_push(skb, ndp_len);
2019 memset(ndp_header, 0, ndp_len);
2020
2021 /* NDP */
2022 put_unaligned_le32(sign, &ndp_header->dwSignature);
2023 put_unaligned_le16(ndp_len, &ndp_header->wLength);
2024
2025 tmp = (void *)ndp_header + opts->ndp_size;
2026 /* (d)wDatagramIndex[0] */
2027 put_mbim(&tmp, opts->dgram_item_len, skb->data + ndp_len - first_skb->data);
2028 /* (d)wDatagramLength[0] */
2029 put_mbim(&tmp, opts->dgram_item_len, skb->len - ndp_len);
2030 /* (d)wDatagramIndex[1] and (d)wDatagramLength[1] already zeroed */
2031 if (unlikely(mbim->is_crc)) {
2032 if (mbim->mode == MODE_MBIM) {
2033 pr_err("MBIM doesn't support CRC!!\n");
2034 } else{
2035 pr_err("NCM fast-path doesn't support CRC in multiple packets!!"
2036 "CI needs to allocate additional space for CRC at the end of the packet.\n");
2037 }
2038 BUG();
2039 }
2040
2041 /* update prev NDP header with new NDP index */
2042 if (first_skb == prev_skb) {
2043 ncb_len = opts->nth_size;
2044 ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
2045 ncb_len += ndp_pad;
2046 pad = ALIGN(ncb_len, div) + rem - ncb_len;
2047 ncb_len += pad;
2048 offset = ncb_len;
2049 }
2050 ndp_header = (void *)(prev_skb->data + offset);
2051 put_mbim_no_inc(NDP_FIELD(ndp_header, wNextFpIndex), opts->next_fp_index, skb->data - first_skb->data);
2052 return skb;
2053}
2054#endif
2055#endif
2056
2057
2058static struct sk_buff *mbim_wrap_ntb(struct gether *port,
2059 struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
2060{
2061 struct f_mbim *mbim = func_to_mbim(&port->func);
2062 struct sk_buff *skb2;
2063 int ncb_len = 0;
2064 int headroom;
2065 __le16 *tmp;
2066 int div = ntb_parameters.wNdpInDivisor;
2067 int rem = ntb_parameters.wNdpInPayloadRemainder;
2068 int pad;
2069 struct nth *nth_header;
2070 struct ndp *ndp_header;
2071 int ndp_align = ntb_parameters.wNdpInAlignment;
2072 int ndp_pad;
2073 unsigned max_size = mbim->port.fixed_in_len;
2074 struct ndp_parser_opts *opts = mbim->parser_opts;
2075 unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
2076 u16 tci = 0;
2077 u8 *c = NULL;
2078 __le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
2079 struct net_device *net_dev = port->ioport->net;
2080
2081 BUG_ON(!skb);
2082 if (skb->len <= ETH_HLEN)
2083 goto error;
2084
2085 /* Reply for outgoing ARP request cause MBIM HOST will NOT answer! */
2086 if (((struct ethhdr *)skb->data)->h_proto == htons(ETH_P_ARP)) {
2087 struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
2088 if (arp->ar_op == htons(ARPOP_REQUEST)) {
2089 unsigned char * arp_ptr = (unsigned char *)(arp + 1);
2090 __be32 dest_ip, src_ip;
2091 pr_info_ratelimited("Reply for outgoing ARP request cause MBIM HOST will NOT answer\n");
2092 arp_ptr += ETH_ALEN;
2093 memcpy(&dest_ip, arp_ptr, 4);
2094 arp_ptr += 4;
2095 arp_ptr += ETH_ALEN;
2096 memcpy(&src_ip, arp_ptr, 4);
2097 skb2 = arp_create(ARPOP_REPLY, ETH_P_ARP, dest_ip, net_dev, src_ip,
2098 net_dev->dev_addr, port->ioport->host_mac, net_dev->dev_addr);
2099 BUG_ON(!skb2);
2100 dev_kfree_skb_any(skb);
2101
2102 skb2->protocol = eth_type_trans(skb2, net_dev);
2103 net_dev->stats.rx_packets++;
2104 net_dev->stats.rx_bytes += skb2->len;
2105
2106 if (netif_rx(skb2) != NET_RX_SUCCESS)
2107 pr_err("Faild to send ARP response to myself\n");
2108 /* This ARP request will be dropped (tx_dropped++) */
2109 return NULL;
2110 }
2111 }
2112
2113 /* mapping VLANs to MBIM sessions:
2114 * no tag => IPS session <0>
2115 * 1 - 255 => IPS session <vlanid>
2116 * 256 - 511 => DSS session <vlanid - 256>
2117 * 512 - 4095 => unsupported, drop
2118 */
2119 if (vlan_get_tag(skb, &tci) >= 0 ) {
2120 switch (tci & 0x0f00) {
2121 case 0x0000: /* VLAN ID 0 - 255 */
2122 break;
2123 case 0x0100: /* VLAN ID 256 - 511 */
2124 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_DSS_SIGN);
2125 break;
2126 default:
2127 pr_err("unsupported tci=0x%04x\n", tci);
2128 goto error;
2129 }
2130
2131 c = (u8 *)&sign;
2132 c[3] = tci;
2133 skb_pull(skb, ETH_HLEN + VLAN_HLEN);
2134 pr_debug("VLAN tci=0x%04x\n", tci);
2135 } else {
2136 skb_pull(skb, ETH_HLEN);
2137 }
2138
2139 ncb_len += opts->nth_size;
2140 ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
2141 ncb_len += ndp_pad;
2142 ncb_len += opts->ndp_size;
2143 ncb_len += 2 * 2 * opts->dgram_item_len; /* Datagram entry */
2144 ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
2145 pad = ALIGN(ncb_len, div) + rem - ncb_len;
2146 ncb_len += pad;
2147
2148 if (ncb_len + skb->len + crc_len > max_size)
2149 goto error;
2150
2151 if (unlikely(skb->cloned || (skb_headroom(skb) < ncb_len)
2152 || (skb_tailroom(skb) < crc_len))) {
2153 skb2 = skb_copy_expand(skb, ncb_len, crc_len, GFP_ATOMIC);
2154 BUG_ON(!skb2);
2155 dev_kfree_skb_any(skb);
2156 skb = skb2;
2157 }
2158
2159 headroom = ncb_len;
2160 // NTH
2161 nth_header = (void *) skb_push(skb, headroom);
2162 memset(nth_header, 0, headroom);
2163
2164 put_unaligned_le32(opts->nth_sign, &nth_header->dwSignature); // dwSignature
2165 put_unaligned_le16(opts->nth_size, &nth_header->wHeaderLength); // wHeaderLength
2166 // skip wSequence
2167 put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->len); //(d)wBlockLength
2168 // the first pointer NTH_FIELD(nth_header, wFpIndex) is right after the NTH + align
2169 put_mbim_no_inc(NTH_FIELD(nth_header, wFpIndex), opts->fp_index, opts->nth_size + ndp_pad); //wNdpIndex(wFpIndex)
2170
2171 // NDP
2172 ndp_header = (void*)nth_header + opts->nth_size + ndp_pad;
2173 put_unaligned_le32(sign, &ndp_header->dwSignature); // dwSignature
2174 put_unaligned_le16(ncb_len - opts->nth_size - pad, &ndp_header->wLength); // wLength
2175
2176 // skip reserved (d)wNextNdpIndex ((d)wNextFpIndex)
2177 tmp += opts->reserved1;
2178 tmp += opts->next_fp_index;
2179 tmp += opts->reserved2;
2180
2181 if (mbim->is_crc) {
2182 pr_err("MBIM doesn't support CRC!!\n");
2183 BUG();
2184 }
2185
2186 tmp = (void *)ndp_header + opts->ndp_size;
2187 put_mbim(&tmp, opts->dgram_item_len, headroom); // (d)wDatagramIndex[0]
2188 put_mbim(&tmp, opts->dgram_item_len, skb->len - headroom); // (d)wDatagramLength[0]
2189 // (d)wDatagramIndex[1] and (d)wDatagramLength[1] already zeroed
2190
2191 return skb;
2192error:
2193 if (skb)
2194 dev_kfree_skb_any(skb);
2195 return NULL;
2196}
2197
2198#ifdef CONFIG_ASR_TOE
2199static struct sk_buff *mbim_wrap_ntb_multipkt(struct gether *port,
2200 struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
2201{
2202 struct sk_buff *first_skb, *prev_skb;
2203
2204 BUG_ON(!aggr_ctx);
2205 BUG_ON(!skb);
2206
2207 if (skb_queue_empty(&aggr_ctx->skb_list) && list_empty(&aggr_ctx->toe_list)) {
2208 skb = mbim_wrap_ntb(port, skb, aggr_ctx);
2209 if (likely(skb)) {
2210 aggr_ctx->total_size = skb->len;
2211 skb_queue_tail(&aggr_ctx->skb_list, skb);
2212 }
2213 aggr_ctx->is_toe = false;
2214 return skb;
2215 }
2216
2217 if (aggr_ctx->is_toe) {
2218 aggr_ctx->pending_skb = skb;
2219 first_skb = (struct sk_buff *)list_first_entry(&aggr_ctx->toe_list, struct toe_pkt_desc, list);
2220 return first_skb;
2221 }
2222
2223 first_skb = skb_peek(&aggr_ctx->skb_list);
2224 prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
2225
2226 BUG_ON(!first_skb);
2227 BUG_ON(!prev_skb);
2228 BUG_ON(first_skb == prev_skb && 1 != skb_queue_len(&aggr_ctx->skb_list));
2229 BUG_ON(!aggr_ctx->total_size);
2230
2231 aggr_ctx->pending_skb = skb;
2232 return first_skb;
2233}
2234
2235static void mbim_add_eth_header(struct gether *port, struct sk_buff *skb)
2236{
2237 struct ethhdr *header;
2238 struct net_device *dev = port->ioport->net;
2239 __be16 proto = htons(ETH_P_802_3);
2240
2241 if (!skb)
2242 return;
2243
2244 if (MBIM_SKB_CB(skb)->tci < 256) { /* IPS session? */
2245 switch (*(skb->data) & 0xf0) {
2246 case 0x40:
2247 proto = htons(ETH_P_IP);
2248 break;
2249 case 0x60:
2250#if 0
2251 if (is_neigh_solicit(buf, len))
2252 do_neigh_solicit(dev, buf, tci);
2253#endif
2254 proto = htons(ETH_P_IPV6);
2255 break;
2256 default:
2257 return;
2258 }
2259 }
2260
2261 if (skb_headroom(skb) < sizeof(*header))
2262 {
2263 pr_err("%s: Headroom is not enough to add eth header\n",__func__);
2264 return;
2265 }
2266 header = (void *)skb_push(skb,sizeof(*header));
2267 memset(header, 0, sizeof(*header));
2268
2269 header->h_proto = proto;
2270 memcpy(header->h_source, port->ioport->host_mac, ETH_ALEN);
2271 memcpy(header->h_dest, dev->dev_addr, ETH_ALEN);
2272#if 0
2273 /* map MBIM session to VLAN */
2274 if (tci)
2275 vlan_put_tag(skb, htons(ETH_P_8021Q), tci);
2276#endif
2277 v7_dma_flush_range((void *)header, (void *)((u32)header + sizeof(*header)));
2278}
2279
2280#ifdef CONFIG_MBIM_INPUT_AS_RNDIS
2281static void mbim_add_rndis_header(struct sk_buff *skb)
2282{
2283 struct rndis_packet_msg_type *header;
2284
2285 if (!skb)
2286 return;
2287 if (skb_headroom(skb) < sizeof(*header))
2288 {
2289 pr_err("%s: Headroom is not enough to add rndis header\n",__func__);
2290 return;
2291 }
2292 header = (void *)skb_push(skb, sizeof(*header));
2293
2294 memset(header, 0, sizeof *header);
2295 header->MessageType = cpu_to_le32(RNDIS_MSG_PACKET);
2296 header->MessageLength = cpu_to_le32(skb->len);
2297 header->DataOffset = cpu_to_le32(36);
2298 header->DataLength = cpu_to_le32(skb->len - sizeof(*header));
2299 v7_dma_flush_range((void *)header, (void *)((u32)header + sizeof(*header)));
2300}
2301#endif
2302
2303#define MBIM_SKB_HEADROOM_RESERVE (64)
2304#define MBIM_SKB_COPY_ALLOC_LENGTH (1514 + MBIM_SKB_HEADROOM_RESERVE + NET_IP_ALIGN)
2305static struct sk_buff *mbim_copy_skb(struct sk_buff *skb, unsigned offset, unsigned len)
2306{
2307 struct sk_buff *skb2;
2308
2309 skb2 = bm_usb_alloc_skb(MBIM_SKB_COPY_ALLOC_LENGTH, GFP_ATOMIC);
2310 if (!skb2)
2311 {
2312 pr_info("%s: No memory, dropping packet.\n",__func__);
2313 return NULL;
2314 }
2315 skb_reserve(skb2, MBIM_SKB_HEADROOM_RESERVE);
2316 memcpy(skb_put(skb2, len), (skb->data + offset), len);
2317
2318 return skb2;
2319}
2320
2321static int mbim_unwrap_ntb(struct gether *port,
2322 struct sk_buff *skb,
2323 struct sk_buff_head *list)
2324{
2325 struct f_mbim *mbim = func_to_mbim(&port->func);
2326 __le16 *tmp = (void *) skb->data;
2327 unsigned index, index2;
2328 unsigned dg_len, dg_len2;
2329 unsigned ndp_len;
2330 struct sk_buff *skb2 = NULL;
2331 struct net_device *net_dev = port->ioport->net;
2332 int ret = -EINVAL;
2333 unsigned max_size = mbim->port.fixed_out_len;
2334 struct ndp_parser_opts *opts = mbim->parser_opts;
2335 int dgram_counter;
2336 u16 tci = 0;
2337 u8 *c = NULL;
2338
2339 /* dwSignature */
2340 if (get_unaligned_le32(tmp) != opts->nth_sign) {
2341 pr_info("Wrong NTH SIGN, skblen %d\n",
2342 skb->len);
2343 print_hex_dump(KERN_INFO, "HEAD:\n", DUMP_PREFIX_ADDRESS, 32, 1,
2344 skb->data, 32, false);
2345 goto err;
2346 }
2347
2348 tmp += 2;
2349 /* wHeaderLength */
2350 if (get_unaligned_le16(tmp++) != opts->nth_size) {
2351 pr_info("Wrong NTB headersize\n");
2352 goto err;
2353 }
2354
2355 tmp++; /* skip wSequence */
2356 /* (d)wBlockLength */
2357 if (get_mbim(&tmp, opts->block_length) > max_size) {
2358 pr_info("OUT size exceeded\n");
2359 goto err;
2360 }
2361
2362 index = get_mbim(&tmp, opts->fp_index);
2363 /* MBIM 3.2 */
2364 if (((index % 4) != 0) && (index < opts->nth_size)) {
2365 pr_info("Bad index: %x\n",
2366 index);
2367 goto err;
2368 }
2369
2370 /* walk through NDP */
2371 tmp = ((void *)skb->data) + index;
2372 switch (get_unaligned_le32(tmp) & cpu_to_le32(0x00ffffff)) {
2373 case USB_CDC_MBIM_NDP16_IPS_SIGN:
2374 case USB_CDC_MBIM_NDP32_IPS_SIGN:
2375 c = (u8 *)tmp;
2376 tci = c[3];
2377 break;
2378 case USB_CDC_MBIM_NDP16_DSS_SIGN:
2379 case USB_CDC_MBIM_NDP32_DSS_SIGN:
2380 c = (u8 *)tmp;
2381 tci = c[3] + 256;
2382 break;
2383 default:
2384 pr_info("unsupported NDP signature <0x%08x>\n", le32_to_cpu(tmp));
2385 goto err;
2386 }
2387
2388 tmp += 2;
2389 ndp_len = get_unaligned_le16(tmp++);
2390 /*
2391 * MBIM 3.3.1
2392 * entry is 2 items
2393 * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
2394 * minimal: struct usb_cdc_mbim_ndpX + normal entry + zero entry
2395 */
2396 if ((ndp_len < opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2))
2397 || (ndp_len % opts->ndplen_align != 0)) {
2398 pr_info("Bad NDP length: %x\n", ndp_len);
2399 goto err;
2400 }
2401 tmp += opts->reserved1;
2402 tmp += opts->next_fp_index; /* skip reserved (d)wNextFpIndex */
2403 tmp += opts->reserved2;
2404
2405 ndp_len -= opts->ndp_size;
2406 index2 = get_mbim(&tmp, opts->dgram_item_len);
2407 dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
2408 dgram_counter = 0;
2409
2410 do {
2411 index = index2;
2412 dg_len = dg_len2;
2413
2414 if (mbim->is_crc) {
2415 pr_err("MBIM doesn't support CRC!!\n");
2416 BUG();
2417 }
2418 index2 = get_mbim(&tmp, opts->dgram_item_len);
2419 dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
2420
2421 if (index2 == 0 || dg_len2 == 0) {
2422 skb2 = skb;
2423 } else {
2424 skb2 = mbim_copy_skb(skb, index, dg_len);
2425 if (skb2 == NULL) {
2426 goto err;
2427 } else {
2428 goto skb_nopull;
2429 }
2430 }
2431
2432 if (!skb_pull(skb2, index)) {
2433 ret = -EOVERFLOW;
2434 goto err;
2435 }
2436
2437skb_nopull:
2438 skb_trim(skb2, dg_len);
2439
2440 skb2->dev = net_dev;
2441 MBIM_SKB_CB(skb2)->sign = MBIM_SK_CB_SIGN;
2442 MBIM_SKB_CB(skb2)->tci = tci;
2443
2444 BUG_ON(tci > 255); //Fix this
2445 /* map MBIM session to VLAN */
2446 if (tci) {//maybe do it always, also for VLAN ID 0
2447 pr_err("Adding VLAN\n");
2448 BUG();
2449 vlan_put_tag(skb2, htons(ETH_P_8021Q), tci);
2450 }
2451
2452 mbim_add_eth_header(port, skb2);
2453#ifdef CONFIG_MBIM_INPUT_AS_RNDIS
2454 mbim_add_rndis_header(skb2);
2455#endif
2456 skb_queue_tail(list, skb2);
2457 ndp_len -= 2 * (opts->dgram_item_len * 2);
2458 dgram_counter++;
2459 if (index2 == 0 || dg_len2 == 0)
2460 break;
2461 } while (ndp_len > 2 * (opts->dgram_item_len * 2)); /* zero entry */
2462 return 0;
2463err:
2464 skb_queue_purge(list);
2465 dev_kfree_skb_any(skb);
2466 return ret;
2467}
2468
2469#else /* CONFIG_ASR_TOE */
2470#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
2471#ifdef CONFIG_USBNET_USE_SG
2472static struct sk_buff *mbim_wrap_ntb_multipkt(struct gether *port,
2473 struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
2474{
2475 struct f_mbim *mbim = func_to_mbim(&port->func);
2476 int aggregate_length, skb_qlen;
2477 struct nth *nth_header;
2478 struct ndp *ndp_header;
2479 int div = ntb_parameters.wNdpInDivisor;
2480 unsigned max_size = mbim->port.fixed_in_len;
2481 struct ndp_parser_opts *opts = mbim->parser_opts;
2482 int skb_len;
2483 int reserve_headroom_size;
2484 int single_entry_ndp_size = opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2);
2485 struct sk_buff *first_skb, *prev_skb;
2486 u16 tci = 0;
2487 u8 *c = NULL;
2488 __le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
2489 __le16 *tmp;
2490 int ndp_len;
2491
2492 BUG_ON(!aggr_ctx);
2493 BUG_ON(!skb);
2494
2495 skb_qlen = skb_queue_len(&aggr_ctx->skb_list);
2496
2497 if (skb_queue_empty(&aggr_ctx->skb_list)) {
2498 skb = mbim_wrap_ntb(port,skb,aggr_ctx);
2499 if (likely(skb)) {
2500 aggr_ctx->total_size = skb->len;
2501 skb_queue_tail(&aggr_ctx->skb_list, skb);
2502 aggr_ctx->num_sgs = 1;
2503 } else {
2504 aggr_ctx->total_size = 0;
2505 aggr_ctx->num_sgs = 0;
2506 pr_err("error: %s 1st pkt failed\n", __func__);
2507 }
2508 return skb;
2509 }
2510
2511 first_skb = skb_peek(&aggr_ctx->skb_list);
2512 prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
2513
2514 BUG_ON(!first_skb);
2515 BUG_ON(!prev_skb);
2516 BUG_ON(first_skb == prev_skb && 1!=skb_queue_len(&aggr_ctx->skb_list));
2517 BUG_ON(!aggr_ctx->total_size);
2518
2519 /* Reply for outgoing ARP request cause MBIM HOST will NOT answer! */
2520 if (((struct ethhdr *)skb->data)->h_proto == htons(ETH_P_ARP)) {
2521 struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
2522 if (arp->ar_op == htons(ARPOP_REQUEST)) {
2523 aggr_ctx->pending_skb = skb;
2524 return first_skb;
2525 }
2526 }
2527
2528 skb_len = skb->len;
2529 if (vlan_get_tag(skb, &tci) >= 0 ) {
2530 skb_len = skb_len - ETH_HLEN - VLAN_HLEN;
2531 reserve_headroom_size = skb_headroom(skb) + ETH_HLEN + VLAN_HLEN;
2532 } else {
2533 skb_len = skb_len - ETH_HLEN;
2534 reserve_headroom_size = skb_headroom(skb) + ETH_HLEN;
2535 }
2536
2537 skb_len += single_entry_ndp_size;
2538 aggregate_length = aggr_ctx->total_size + skb_len;
2539
2540 if((ALIGN(skb_len, div) - skb_len) > 0 ||
2541 (ALIGN(prev_skb->len, div) - prev_skb->len) > 0) {
2542 aggr_ctx->pending_skb = skb;
2543 return first_skb;
2544 }
2545
2546 /* mbim aggregation condition */
2547 if (aggregate_length > max_size ||
2548 (mbim->ntb_max_datagrams != 0 && mbim->ntb_max_datagrams <= skb_qlen) ||
2549 skb_qlen >= MBIM_MAX_DGRAMS_AGGREGATION ||
2550 skb_cloned(skb) ||
2551 reserve_headroom_size < single_entry_ndp_size ||
2552 aggr_ctx->num_sgs >= (USBNET_SG_NENTS - 1)) {
2553 aggr_ctx->pending_skb = skb;
2554 return first_skb;
2555 }
2556
2557 /* mapping VLANs to MBIM sessions:
2558 * no tag => IPS session <0>
2559 * 1 - 255 => IPS session <vlanid>
2560 * 256 - 511 => DSS session <vlanid - 256>
2561 * 512 - 4095 => unsupported, drop
2562 */
2563 if (vlan_get_tag(skb, &tci) >= 0 ) {
2564 switch (tci & 0x0f00) {
2565 case 0x0000: /* VLAN ID 0 - 255 */
2566 break;
2567 case 0x0100: /* VLAN ID 256 - 511 */
2568 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_DSS_SIGN);
2569 break;
2570 default:
2571 pr_err("unsupported tci=0x%04x\n", tci);
2572 goto error;
2573 }
2574 c = (u8 *)&sign;
2575 c[3] = tci;
2576 skb_pull(skb, ETH_HLEN + VLAN_HLEN);
2577 pr_debug("VLAN tci=0x%04x\n", tci);
2578 } else {
2579 skb_pull(skb, ETH_HLEN);
2580 }
2581
2582 // update NDP
2583 ndp_len = opts->ndp_size;
2584 ndp_len += 2 * 2 * opts->dgram_item_len; /* Datagram entry */
2585 ndp_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
2586 ndp_header = (void *)skb_push(skb, ndp_len);
2587 memset(ndp_header, 0, ndp_len);
2588
2589 // dwSignature and wLength
2590 put_unaligned_le32(sign, &ndp_header->dwSignature);
2591 put_unaligned_le16(ndp_len, &ndp_header->wLength);
2592
2593 tmp = (void *)ndp_header + opts->ndp_size;
2594 // (d)wDatagramIndex[0] and (d)wDatagramLength[0]
2595 put_mbim(&tmp, opts->dgram_item_len, aggr_ctx->total_size + ndp_len);
2596 put_mbim(&tmp, opts->dgram_item_len, skb->len - ndp_len);
2597 /* (d)wDatagramIndex[1] and (d)wDatagramLength[1] already zeroed */
2598 if (unlikely(mbim->is_crc)) {
2599 pr_err("MBIM doesn't support CRC!!\n");
2600 BUG();
2601 }
2602
2603 // update wNextFpIndex(wNextNdpIndex) of prev ntb
2604 if (first_skb == prev_skb) {
2605 ndp_header = (void *)prev_skb->data + opts->nth_size;
2606 } else {
2607 ndp_header = (void *)prev_skb->data;
2608 }
2609 put_mbim_no_inc(NDP_FIELD(ndp_header, wNextFpIndex), opts->next_fp_index, aggr_ctx->total_size);
2610 skb_queue_tail(&aggr_ctx->skb_list, skb);
2611
2612 aggr_ctx->total_size += skb->len;
2613 aggr_ctx->num_sgs++;
2614
2615 // update NTH
2616 nth_header = (void *)first_skb->data;
2617 // (d)wBlockLength
2618 put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, aggr_ctx->total_size);
2619
2620 return first_skb;
2621
2622error:
2623 aggr_ctx->pending_skb = skb;
2624 return first_skb;
2625}
2626#else
2627static struct sk_buff *mbim_wrap_ntb_multipkt(struct gether *port,
2628 struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
2629{
2630 struct f_mbim *mbim = func_to_mbim(&port->func);
2631 int padding, aggregate_length, skb_qlen;
2632 struct nth *nth_header;
2633 unsigned max_size = mbim->port.fixed_in_len;
2634 struct ndp_parser_opts *opts = mbim->parser_opts;
2635 int single_entry_ndp_size = opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2);
2636 struct sk_buff *first_skb, *prev_skb;
2637 u16 tci = 0;
2638 u8 *c = NULL;
2639 __le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
2640
2641 BUG_ON(!aggr_ctx);
2642 BUG_ON(!skb);
2643
2644 skb_qlen = skb_queue_len(&aggr_ctx->skb_list);
2645
2646 if (skb_queue_empty(&aggr_ctx->skb_list)) {
2647 skb = mbim_wrap_ntb(port,skb,aggr_ctx);
2648 if (likely(skb)) {
2649 aggr_ctx->total_size = skb->len;
2650 skb_queue_tail(&aggr_ctx->skb_list, skb);
2651 }
2652 return skb;
2653 }
2654
2655 first_skb = skb_peek(&aggr_ctx->skb_list);
2656 prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
2657
2658 BUG_ON(!first_skb);
2659 BUG_ON(!prev_skb);
2660 BUG_ON(first_skb == prev_skb && 1!=skb_queue_len(&aggr_ctx->skb_list));
2661 BUG_ON(!aggr_ctx->total_size);
2662
2663 /* Reply for outgoing ARP request cause MBIM HOST will NOT answer! */
2664 if (((struct ethhdr *)skb->data)->h_proto == htons(ETH_P_ARP)) {
2665 struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
2666 if (arp->ar_op == htons(ARPOP_REQUEST)) {
2667 aggr_ctx->pending_skb = skb;
2668 return first_skb;
2669 }
2670 }
2671
2672 padding = (unsigned)skb->data - single_entry_ndp_size - (unsigned)prev_skb->tail;
2673 aggregate_length = aggr_ctx->total_size + skb->len + padding + single_entry_ndp_size;
2674 BUG_ON(aggregate_length != skb->tail - first_skb->data);
2675
2676 if (vlan_get_tag(skb, &tci) >= 0 ) {
2677 padding = padding + ETH_HLEN + VLAN_HLEN;
2678 } else {
2679 padding = padding + ETH_HLEN;
2680 }
2681
2682 /* mbim aggregation condition */
2683 if (aggregate_length > max_size ||
2684 (mbim->ntb_max_datagrams != 0 && mbim->ntb_max_datagrams <= skb_qlen) ||
2685 skb_qlen >= MBIM_MAX_DGRAMS_AGGREGATION ||
2686 skb_cloned(skb) ||
2687 skb_headroom(skb) < single_entry_ndp_size ||
2688 padding > AGGR_MAX_PADDING ||
2689 padding < 0) {
2690 aggr_ctx->pending_skb = skb;
2691 return first_skb;
2692 }
2693
2694 /* mapping VLANs to MBIM sessions:
2695 * no tag => IPS session <0>
2696 * 1 - 255 => IPS session <vlanid>
2697 * 256 - 511 => DSS session <vlanid - 256>
2698 * 512 - 4095 => unsupported, drop
2699 */
2700 if (vlan_get_tag(skb, &tci) >= 0 ) {
2701 switch (tci & 0x0f00) {
2702 case 0x0000: /* VLAN ID 0 - 255 */
2703 break;
2704 case 0x0100: /* VLAN ID 256 - 511 */
2705 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_DSS_SIGN);
2706 break;
2707 default:
2708 pr_err("unsupported tci=0x%04x\n", tci);
2709 goto error;
2710 }
2711 c = (u8 *)&sign;
2712 c[3] = tci;
2713 skb_pull(skb, ETH_HLEN + VLAN_HLEN);
2714 pr_debug("VLAN tci=0x%04x\n", tci);
2715 } else {
2716 skb_pull(skb, ETH_HLEN);
2717 }
2718
2719 // update NTH
2720 nth_header = (void *)first_skb->data;
2721 // (d)wBlockLength
2722 put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->tail - first_skb->data);
2723 add_ndp_header(port, skb, aggr_ctx, sign);
2724
2725 skb_queue_tail(&aggr_ctx->skb_list, skb);
2726 BUG_ON(skb->tail - first_skb->data != aggr_ctx->total_size + skb->len + padding);
2727 aggr_ctx->total_size = skb->tail - first_skb->data;
2728 return first_skb;
2729error:
2730 aggr_ctx->pending_skb = skb;
2731 return first_skb;
2732}
2733#endif //CONFIG_USBNET_USE_SG
2734#endif //CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
2735
2736static int mbim_unwrap_ntb(struct gether *port,
2737 struct sk_buff *skb,
2738 struct sk_buff_head *list)
2739{
2740 struct f_mbim *mbim = func_to_mbim(&port->func);
2741 __le16 *tmp = (void *) skb->data;
2742 unsigned index, index2;
2743 unsigned dg_len, dg_len2;
2744 unsigned ndp_len;
2745 struct sk_buff *skb2 = NULL;
2746 struct net_device *net_dev = port->ioport->net;
2747 int ret = -EINVAL;
2748 unsigned max_size = mbim->port.fixed_out_len;
2749 struct ndp_parser_opts *opts = mbim->parser_opts;
2750 unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
2751 int dgram_counter;
2752 u16 tci = 0;
2753 u8 *c = NULL;
2754
2755 /* dwSignature */
2756 if (get_unaligned_le32(tmp) != opts->nth_sign) {
2757 pr_info("Wrong NTH SIGN, skblen %d\n",
2758 skb->len);
2759 print_hex_dump(KERN_INFO, "HEAD:\n", DUMP_PREFIX_ADDRESS, 32, 1,
2760 skb->data, 32, false);
2761
2762 goto err;
2763 }
2764 tmp += 2;
2765 /* wHeaderLength */
2766 if (get_unaligned_le16(tmp++) != opts->nth_size) {
2767 pr_info("Wrong NTB headersize\n");
2768 goto err;
2769 }
2770 tmp++; /* skip wSequence */
2771
2772 /* (d)wBlockLength */
2773 if (get_mbim(&tmp, opts->block_length) > max_size) {
2774 pr_info("OUT size exceeded\n");
2775 goto err;
2776 }
2777
2778 index = get_mbim(&tmp, opts->fp_index);
2779 /* MBIM 3.2 */
2780 if (((index % 4) != 0) && (index < opts->nth_size)) {
2781 pr_info("Bad index: %x\n",
2782 index);
2783 goto err;
2784 }
2785
2786 /* walk through NDP */
2787 tmp = ((void *)skb->data) + index;
2788 switch (get_unaligned_le32(tmp) & cpu_to_le32(0x00ffffff)) {
2789 case USB_CDC_MBIM_NDP16_IPS_SIGN:
2790 case USB_CDC_MBIM_NDP32_IPS_SIGN:
2791 c = (u8 *)tmp;
2792 tci = c[3];
2793 break;
2794 case USB_CDC_MBIM_NDP16_DSS_SIGN:
2795 case USB_CDC_MBIM_NDP32_DSS_SIGN:
2796 c = (u8 *)tmp;
2797 tci = c[3] + 256;
2798 break;
2799 default:
2800 pr_info("unsupported NDP signature <0x%08x>\n", le32_to_cpu(tmp));
2801 goto err;
2802 }
2803
2804 tmp += 2;
2805
2806 ndp_len = get_unaligned_le16(tmp++);
2807 /*
2808 * MBIM 3.3.1
2809 * entry is 2 items
2810 * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
2811 * minimal: struct usb_cdc_mbim_ndpX + normal entry + zero entry
2812 */
2813 if ((ndp_len < opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2))
2814 || (ndp_len % opts->ndplen_align != 0)) {
2815 pr_info("Bad NDP length: %x\n", ndp_len);
2816 goto err;
2817 }
2818 tmp += opts->reserved1;
2819 tmp += opts->next_fp_index; /* skip reserved (d)wNextFpIndex */
2820 tmp += opts->reserved2;
2821
2822 ndp_len -= opts->ndp_size;
2823 index2 = get_mbim(&tmp, opts->dgram_item_len);
2824 dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
2825 dgram_counter = 0;
2826
2827 do {
2828 index = index2;
2829 dg_len = dg_len2;
2830 if (dg_len < 14 + crc_len) { /* ethernet header + crc */
2831 pr_info("Bad dgram length: %x\n",
2832 dg_len);
2833 goto err;
2834 }
2835 if (mbim->is_crc) {
2836 uint32_t crc, crc2;
2837
2838 crc = get_unaligned_le32(skb->data +
2839 index + dg_len - crc_len);
2840 crc2 = ~crc32_le(~0,
2841 skb->data + index,
2842 dg_len - crc_len);
2843 if (crc != crc2) {
2844 pr_info("Bad CRC\n");
2845 goto err;
2846 }
2847 }
2848
2849 index2 = get_mbim(&tmp, opts->dgram_item_len);
2850 dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
2851
2852 if (index2 == 0 || dg_len2 == 0) {
2853 skb2 = skb;
2854 } else {
2855 skb2 = skb_clone(skb, GFP_ATOMIC);
2856 BUG_ON(!skb2);
2857 }
2858
2859 if (!skb_pull(skb2, index)) {
2860 ret = -EOVERFLOW;
2861 goto err;
2862 }
2863
2864 skb_trim(skb2, dg_len - crc_len);
2865 skb2->dev = net_dev;
2866
2867 MBIM_SKB_CB(skb2)->sign = MBIM_SK_CB_SIGN;
2868 MBIM_SKB_CB(skb2)->tci = tci;
2869
2870 BUG_ON(tci > 255); //Fix this
2871 /* map MBIM session to VLAN */
2872 if (tci) {//maybe do it always, also for VLAN ID 0
2873 pr_err("Adding VLAN\n");
2874 BUG();
2875 vlan_put_tag(skb2, htons(ETH_P_8021Q), tci);
2876 }
2877
2878 skb_queue_tail(list, skb2);
2879
2880 ndp_len -= 2 * (opts->dgram_item_len * 2);
2881
2882 dgram_counter++;
2883
2884 if (index2 == 0 || dg_len2 == 0)
2885 break;
2886 } while (ndp_len > 2 * (opts->dgram_item_len * 2)); /* zero entry */
2887
2888 return 0;
2889err:
2890 skb_queue_purge(list);
2891 dev_kfree_skb_any(skb);
2892 return ret;
2893}
2894
2895static struct sk_buff *mbim_process_dgram(struct gether *port,
2896 struct sk_buff *skb)
2897{
2898 struct net_device *dev = port->ioport->net;
2899 struct sk_buff *skb2 = NULL;
2900 __be16 proto = htons(ETH_P_802_3);
2901
2902 BUG_ON(!dev || !port || !skb || !skb->data || !skb->len);
2903
2904
2905
2906 if (MBIM_SKB_CB(skb)->tci < 256) { /* IPS session? */
2907
2908 switch (*(skb->data) & 0xf0) {
2909 case 0x40:
2910 proto = htons(ETH_P_IP);
2911 break;
2912 case 0x60:
2913#if 0
2914 if (is_neigh_solicit(buf, len))
2915 do_neigh_solicit(dev, buf, tci);
2916#endif
2917 proto = htons(ETH_P_IPV6);
2918 break;
2919 default:
2920 goto err;
2921 }
2922 }
2923
2924 skb2 = netdev_alloc_skb_ip_align(dev, skb->len + ETH_HLEN);
2925 if (!skb2)
2926 goto err;
2927
2928 /* add an ethernet header */
2929 skb_put(skb2, ETH_HLEN);
2930 skb_reset_mac_header(skb2);
2931 eth_hdr(skb2)->h_proto = proto;
2932
2933 memcpy(eth_hdr(skb2)->h_source, port->ioport->host_mac, ETH_ALEN);
2934 memcpy(eth_hdr(skb2)->h_dest, dev->dev_addr, ETH_ALEN);
2935
2936 /* add datagram */
2937 memcpy(skb_put(skb2, skb->len), skb->data, skb->len);
2938#if 0
2939 /* map MBIM session to VLAN */
2940 if (tci)
2941 vlan_put_tag(skb, htons(ETH_P_8021Q), tci);
2942#endif
2943err:
2944 return skb2;
2945}
2946#endif /* CONFIG_ASR_TOE */
2947
2948static void mbim_disable(struct usb_function *f)
2949{
2950 struct f_mbim *mbim = func_to_mbim(f);
2951
2952 pr_info("SET DEVICE OFFLINE\n");
2953#ifdef CONFIG_ASR_TOE
2954 mbim->port.ueth_type = UETHER_UNKNOWN;
2955#endif
2956 atomic_set(&mbim->online, 0);
2957 atomic_set(&mbim->response_available, 0);
2958 atomic_set(&mbim->notify_count, 0);
2959
2960 mbim_clear_queues(mbim);
2961 mbim_reset_function_queue(mbim);
2962
2963 if (mbim->port.in_ep->driver_data)
2964 gether_disconnect(&mbim->port);
2965
2966 if (mbim->notify->driver_data) {
2967 usb_ep_disable(mbim->notify);
2968 mbim->notify->driver_data = NULL;
2969 mbim->notify_desc = NULL;
2970 }
2971}
2972
2973
2974static struct sk_buff *ncm_wrap_ntb(struct gether *port,
2975 struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
2976{
2977 struct f_mbim *mbim = func_to_mbim(&port->func);
2978 struct sk_buff *skb2;
2979 int ncb_len, headroom;
2980 int div = ntb_parameters.wNdpInDivisor;
2981 int rem = ntb_parameters.wNdpInPayloadRemainder;
2982 int pad = 0;
2983 struct nth *nth_header;
2984 struct ndp *ndp_header;
2985 __le16 *tmp;
2986 int ndp_align = ntb_parameters.wNdpInAlignment;
2987 int ndp_pad;
2988 unsigned max_size = mbim->port.fixed_in_len;
2989 struct ndp_parser_opts *opts = mbim->parser_opts;
2990 unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
2991
2992 ncb_len = opts->nth_size;
2993 ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
2994 ncb_len += ndp_pad;
2995 ncb_len += opts->ndp_size;
2996 ncb_len += 2 * 2 * opts->dgram_item_len; /* Datagram entry */
2997 ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
2998 pad = ALIGN(ncb_len, div) + rem - ncb_len;
2999 ncb_len += pad;
3000
3001 if (ncb_len + skb->len + crc_len > max_size) {
3002 dev_kfree_skb_any(skb);
3003 return NULL;
3004 }
3005
3006 if (unlikely(skb->cloned || skb_headroom(skb) < ncb_len
3007 || skb_tailroom(skb) < crc_len)) {
3008 skb2 = skb_copy_expand(skb, ncb_len,
3009 crc_len,
3010 GFP_ATOMIC);
3011 dev_kfree_skb_any(skb);
3012 if (!skb2)
3013 return NULL;
3014
3015 skb = skb2;
3016 skb2 = NULL;
3017 }
3018
3019#ifndef CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS
3020 headroom = skb_headroom(skb);
3021#ifndef CONFIG_ASR_TOE
3022 if (headroom >= (ncb_len + 4 * (MAX_IN_SKB_NUM + 1))) {
3023 headroom = (ncb_len + 4 * MAX_IN_SKB_NUM);
3024 headroom += ((u32)(skb->data - headroom) & 0x3);
3025 }
3026#endif
3027#else
3028 headroom = ncb_len;
3029#endif
3030 nth_header = (void *)skb_push(skb, headroom);
3031 memset(nth_header, 0, headroom);
3032 put_unaligned_le32(opts->nth_sign, &nth_header->dwSignature);
3033 put_unaligned_le16(opts->nth_size, &nth_header->wHeaderLength);
3034 put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->len); /* the first pointer is right after the NTH + align */
3035 put_mbim_no_inc(NTH_FIELD(nth_header, wFpIndex), opts->fp_index, opts->nth_size + ndp_pad); /* NDP */
3036 ndp_header = (void *)nth_header + opts->nth_size + ndp_pad;
3037 put_unaligned_le32(opts->ndp_sign, &ndp_header->dwSignature);
3038 put_unaligned_le16(ncb_len - opts->nth_size, &ndp_header->wLength);
3039 if (unlikely(mbim->is_crc)) {
3040 pr_err("NCM fast-path doesn't support CRC in multiple packets!!"
3041 "CI needs to allocate additional space for CRC at the end of the packet.\n");
3042 BUG();
3043 }
3044 tmp = (void *)ndp_header + opts->ndp_size; /* (d)wDatagramIndex[0] */
3045 put_mbim(&tmp, opts->dgram_item_len, headroom); /* (d)wDatagramLength[0] */
3046 put_mbim(&tmp, opts->dgram_item_len, skb->len - headroom); /* (d)wDatagramIndex[1] and (d)wDatagramLength[1] already zeroed */
3047 return skb;
3048}
3049
3050#ifdef CONFIG_ASR_TOE
3051static struct sk_buff *ncm_wrap_ntb_multipkt(struct gether *port,
3052 struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
3053{
3054 struct f_mbim *mbim = func_to_mbim(&port->func);
3055 int ndp_align = ntb_parameters.wNdpInAlignment;
3056 int ndp_pad;
3057 struct ndp_parser_opts *opts = mbim->parser_opts;
3058 int ncb_len;
3059 struct sk_buff *first_skb, *prev_skb;
3060 unsigned int max_nth_size, ndp_entry_pointer_len;
3061
3062 ncb_len = opts->nth_size;
3063 ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
3064 ncb_len += ndp_pad;
3065 ncb_len += opts->ndp_size;
3066 ndp_entry_pointer_len = 2 * 2 * opts->dgram_item_len; /* Datagram entry */
3067 ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
3068
3069 BUG_ON(!aggr_ctx);
3070 BUG_ON(!skb);
3071
3072 if (skb_queue_empty(&aggr_ctx->skb_list) && list_empty(&aggr_ctx->toe_list)) {
3073 struct sk_buff *old_skb = skb; /*check instead in aggregation condition that mem is shmem */
3074 max_nth_size = skb_headroom(skb);
3075 skb = ncm_wrap_ntb(port,skb,aggr_ctx);
3076 if (likely(skb)) {
3077 /*save first skb headroom for future aggregations limit*/
3078 NTB_MAX_HEADER_SIZE(skb) = old_skb==skb ? max_nth_size : 0;
3079 aggr_ctx->total_size = skb->len;
3080 skb_queue_tail(&aggr_ctx->skb_list, skb);
3081 }
3082 aggr_ctx->is_toe = false;
3083 return skb;
3084 }
3085
3086 if (aggr_ctx->is_toe) {
3087 aggr_ctx->pending_skb = skb;
3088 first_skb = (struct sk_buff *)list_first_entry(&aggr_ctx->toe_list, struct toe_pkt_desc, list);
3089 return first_skb;
3090 }
3091
3092 first_skb = skb_peek(&aggr_ctx->skb_list);
3093 prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
3094
3095 BUG_ON(!first_skb);
3096 BUG_ON(!prev_skb);
3097 BUG_ON(first_skb == prev_skb && 1 != skb_queue_len(&aggr_ctx->skb_list));
3098 BUG_ON(!aggr_ctx->total_size);
3099
3100 aggr_ctx->pending_skb = skb;
3101 return first_skb;
3102}
3103
3104#define NCM_LONG_SKB_LEN (1000)
3105#define SKB_HEADROOM_RESERVE (64)
3106#define SKB_COPY_ALLOC_LENGTH (1514 + SKB_HEADROOM_RESERVE + NET_IP_ALIGN)
3107
3108#ifdef CONFIG_NCM_INPUT_AS_RNDIS
3109static void ncm_add_rndis_header(struct sk_buff *skb)
3110{
3111 struct rndis_packet_msg_type *header;
3112
3113 if (!skb)
3114 return;
3115 header = (void *)skb_push(skb, sizeof(*header));
3116 memset(header, 0, sizeof *header);
3117 header->MessageType = cpu_to_le32(RNDIS_MSG_PACKET);
3118 header->MessageLength = cpu_to_le32(skb->len);
3119 header->DataOffset = cpu_to_le32(36);
3120 header->DataLength = cpu_to_le32(skb->len - sizeof(*header));
3121 v7_dma_flush_range((void *)header, (void *)((u32)header + sizeof(*header)));
3122}
3123#endif
3124
3125/* only short skb is copy by SW or DMA */
3126static struct sk_buff *ncm_copy_skb(struct sk_buff *skb, unsigned offset, unsigned len)
3127{
3128 struct sk_buff *skb2;
3129
3130 skb2 = bm_usb_alloc_skb(SKB_COPY_ALLOC_LENGTH, GFP_ATOMIC);
3131 if (!skb2)
3132 return NULL;
3133
3134 skb_reserve(skb2, SKB_HEADROOM_RESERVE);
3135 memcpy(skb_put(skb2, len), (skb->data + offset), len);
3136
3137 return skb2;
3138}
3139
3140static int ncm_unwrap_ntb(struct gether *port,
3141 struct sk_buff *skb,
3142 struct sk_buff_head *list)
3143{
3144 struct f_mbim *mbim = func_to_mbim(&port->func);
3145 __le16 *tmp = (void *) skb->data;
3146 unsigned index, index2;
3147 unsigned dg_len, dg_len2;
3148 unsigned ndp_len;
3149 struct sk_buff *skb2;
3150 int ret = -EINVAL;
3151 unsigned max_size = le32_to_cpu(ntb_parameters.dwNtbOutMaxSize);
3152 struct ndp_parser_opts *opts = mbim->parser_opts;
3153 unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
3154 int dgram_counter, nocopy_dgram_counter;
3155 bool skb_meet_nocopy = false;
3156 unsigned nocopy_offset, nocopy_len;
3157
3158
3159 /* dwSignature */
3160 if (get_unaligned_le32(tmp) != opts->nth_sign) {
3161 pr_err("Wrong NTH SIGN, skblen %d\n",
3162 skb->len);
3163 print_hex_dump(KERN_INFO, "HEAD:\n", DUMP_PREFIX_ADDRESS, 32, 1,
3164 skb->data, 32, false);
3165
3166 goto err;
3167 }
3168 tmp += 2;
3169 /* wHeaderLength */
3170 if (get_unaligned_le16(tmp++) != opts->nth_size) {
3171 pr_err("Wrong NTB headersize\n");
3172 goto err;
3173 }
3174 tmp++; /* skip wSequence */
3175
3176 /* (d)wBlockLength */
3177 if (get_mbim(&tmp, opts->block_length) > max_size) {
3178 pr_err("OUT size exceeded\n");
3179 goto err;
3180 }
3181
3182 index = get_mbim(&tmp, opts->fp_index);
3183 /* NCM 3.2 */
3184 if (((index % 4) != 0) && (index < opts->nth_size)) {
3185 pr_err("Bad index: %x\n",
3186 index);
3187 goto err;
3188 }
3189
3190 /* walk through NDP */
3191 tmp = ((void *)skb->data) + index;
3192 if (get_unaligned_le32(tmp) != opts->ndp_sign) {
3193 pr_err("Wrong NDP SIGN\n");
3194 goto err;
3195 }
3196 tmp += 2;
3197
3198 ndp_len = get_unaligned_le16(tmp++);
3199 /*
3200 * NCM 3.3.1
3201 * entry is 2 items
3202 * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
3203 * minimal: struct usb_cdc_ncm_ndpX + normal entry + zero entry
3204 */
3205 if ((ndp_len < opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2))
3206 || (ndp_len % opts->ndplen_align != 0)) {
3207 pr_err("Bad NDP length: %x\n", ndp_len);
3208 goto err;
3209 }
3210 tmp += opts->reserved1;
3211 tmp += opts->next_fp_index; /* skip reserved (d)wNextFpIndex */
3212 tmp += opts->reserved2;
3213
3214 ndp_len -= opts->ndp_size;
3215 index2 = get_mbim(&tmp, opts->dgram_item_len);
3216 dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
3217 dgram_counter = 0;
3218
3219 do {
3220 index = index2;
3221 dg_len = dg_len2;
3222
3223 if (!skb_meet_nocopy && dg_len > NCM_LONG_SKB_LEN) {
3224 skb_meet_nocopy = true;
3225 nocopy_offset = index;
3226 nocopy_len = dg_len;
3227 nocopy_dgram_counter = dgram_counter;
3228 }
3229
3230 if (dg_len < 14 + crc_len) { /* ethernet header + crc */
3231 pr_err("Bad dgram length: %x\n",
3232 dg_len);
3233 goto err;
3234 }
3235 if (mbim->is_crc) {
3236 uint32_t crc, crc2;
3237
3238 crc = get_unaligned_le32(skb->data +
3239 index + dg_len - crc_len);
3240 crc2 = ~crc32_le(~0,
3241 skb->data + index,
3242 dg_len - crc_len);
3243 if (crc != crc2) {
3244 pr_err("Bad CRC\n");
3245 goto err;
3246 }
3247 }
3248
3249 index2 = get_mbim(&tmp, opts->dgram_item_len);
3250 dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
3251
3252 /* last skb */
3253 if (index2 == 0 || dg_len2 == 0) {
3254 /* only one skb, no copy */
3255 if (dgram_counter == 0) {
3256 skb2 = skb;
3257 goto skb_nocpy_pull;
3258 } else {
3259 if (skb_meet_nocopy) {
3260 /* last skb is no cpy */
3261 if (nocopy_dgram_counter == (dgram_counter - 1)) {
3262 skb2 = skb;
3263 goto skb_nocpy_pull;
3264 } else {
3265 /* last pkt need to be copied
3266 * handle the last skb and non-copy skb
3267 */
3268 skb2 = ncm_copy_skb(skb, index, dg_len);
3269 if (skb2 == NULL) {
3270 pr_info("%s: skb cp err\n", __func__);
3271 goto err;
3272 } else {
3273 skb_trim(skb2, dg_len - crc_len);
3274 skb_queue_tail(list, skb2);
3275
3276 /* handle the non copy skb */
3277 skb2 = skb;
3278 index = nocopy_offset;
3279 dg_len = nocopy_len;
3280 goto skb_nocpy_pull;
3281 }
3282 }
3283 } else {
3284 skb2 = skb;
3285 goto skb_nocpy_pull;
3286 }
3287 }
3288 } else {
3289 if (skb_meet_nocopy && nocopy_dgram_counter == dgram_counter) {
3290 /* TODO: skip the skb copy, do nothing? */
3291 goto skip_skb_copy;
3292 } else {
3293 skb2 = ncm_copy_skb(skb, index, dg_len);
3294 if (skb2 == NULL) {
3295 pr_info("%s: skb cp err2\n", __func__);
3296 goto err;
3297 } else {
3298 goto skb_nopull;
3299 }
3300 }
3301 }
3302
3303skb_nocpy_pull:
3304 if (!skb_pull(skb2, index)) {
3305 ret = -EOVERFLOW;
3306 pr_err("%s: skb2 pull error\n", __func__);
3307 goto err;
3308 }
3309
3310skb_nopull:
3311 skb_trim(skb2, dg_len - crc_len);
3312#ifdef CONFIG_NCM_INPUT_AS_RNDIS
3313 ncm_add_rndis_header(skb2);
3314#endif
3315 skb_queue_tail(list, skb2);
3316
3317skip_skb_copy:
3318 ndp_len -= 2 * (opts->dgram_item_len * 2);
3319
3320 dgram_counter++;
3321
3322 if (index2 == 0 || dg_len2 == 0)
3323 break;
3324 } while (ndp_len > 2 * (opts->dgram_item_len * 2)); /* zero entry */
3325
3326 VDBG(port->func.config->cdev,
3327 "Parsed NTB with %d frames\n", dgram_counter);
3328 return 0;
3329err:
3330 skb_queue_purge(list);
3331 dev_kfree_skb_any(skb);
3332 return ret;
3333}
3334
3335#else
3336#ifdef CONFIG_USB_G_NCM_MULT_PKT_SUPPORT
3337#ifdef CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS
3338static struct sk_buff *ncm_wrap_ntb_multipkt(struct gether *port,
3339 struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
3340{
3341 struct f_mbim *mbim = func_to_mbim(&port->func);
3342 int padding, aggregate_length, skb_qlen;
3343 struct nth *nth_header;
3344 int ndp_align = ntb_parameters.wNdpInAlignment;
3345 int ndp_pad;
3346 unsigned max_size = mbim->port.fixed_in_len;
3347 struct ndp_parser_opts *opts = mbim->parser_opts;
3348 int ncb_len;
3349 int single_entry_ndp_size = opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2);
3350 struct sk_buff *first_skb, *prev_skb;
3351
3352 BUG_ON(!aggr_ctx);
3353 BUG_ON(!skb);
3354
3355 skb_qlen = skb_queue_len(&aggr_ctx->skb_list);
3356 ncb_len = opts->nth_size;
3357 ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
3358 ncb_len += ndp_pad;
3359 ncb_len += opts->ndp_size;
3360 ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
3361
3362 if (skb_queue_empty(&aggr_ctx->skb_list)) {
3363 skb = ncm_wrap_ntb(port,skb,aggr_ctx);
3364 if (likely(skb)) {
3365 aggr_ctx->total_size = skb->len;
3366 skb_queue_tail(&aggr_ctx->skb_list, skb);
3367 }
3368 return skb;
3369 }
3370
3371 first_skb = skb_peek(&aggr_ctx->skb_list);
3372 prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
3373
3374 BUG_ON(!first_skb);
3375 BUG_ON(!prev_skb);
3376 BUG_ON(first_skb == prev_skb && 1!=skb_queue_len(&aggr_ctx->skb_list));
3377 BUG_ON(!aggr_ctx->total_size);
3378 padding = (unsigned)skb->data - single_entry_ndp_size - (unsigned)prev_skb->tail;
3379 aggregate_length = aggr_ctx->total_size + skb->len + padding + single_entry_ndp_size;
3380 BUG_ON(aggregate_length != skb->tail - first_skb->data);
3381
3382 /* ncm aggregation condition */
3383 if (aggregate_length > max_size ||
3384 (mbim->ntb_max_datagrams != 0 && mbim->ntb_max_datagrams <= skb_qlen) ||
3385 skb_qlen >= MAX_DGRAMS_AGGREGATION ||
3386 skb_cloned(skb) ||
3387 skb_headroom(skb) < single_entry_ndp_size ||
3388 padding > AGGR_MAX_PADDING ||
3389 padding < 0) {
3390 aggr_ctx->pending_skb = skb;
3391 return first_skb;
3392 }
3393
3394 /* update NTH */
3395 nth_header = (void *)first_skb->data;
3396 /* (d)wBlockLength */
3397 put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->tail - first_skb->data);
3398
3399 add_ndp_header(port, skb, aggr_ctx, opts->ndp_sign);
3400
3401 skb_queue_tail(&aggr_ctx->skb_list, skb);
3402 BUG_ON(skb->tail - first_skb->data != aggr_ctx->total_size + skb->len + padding);
3403 aggr_ctx->total_size = skb->tail - first_skb->data;
3404 /* aggr_ctx->total_padding += padding;*/
3405 return first_skb;
3406}
3407#else
3408static struct sk_buff *ncm_wrap_ntb_multipkt(struct gether *port,
3409 struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
3410{
3411 struct f_mbim *mbim = func_to_mbim(&port->func);
3412 int padding, aggregate_length, skb_qlen;
3413 __le16 *tmp;
3414 int ndp_align = ntb_parameters.wNdpInAlignment;
3415 int ndp_pad;
3416 struct nth *nth_header;
3417 struct ndp *ndp_header;
3418 unsigned max_size = mbim->port.fixed_in_len;
3419 struct ndp_parser_opts *opts = mbim->parser_opts;
3420 int ncb_len;
3421 struct sk_buff *first_skb, *prev_skb;
3422 unsigned int max_nth_size, ndp_entry_pointer_len;
3423
3424 ncb_len = opts->nth_size;
3425 ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
3426 ncb_len += ndp_pad;
3427 ncb_len += opts->ndp_size;
3428 ndp_entry_pointer_len = 2 * 2 * opts->dgram_item_len; /* Datagram entry */
3429 ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
3430
3431 BUG_ON(!aggr_ctx);
3432 BUG_ON(!skb);
3433
3434 if (skb_queue_empty(&aggr_ctx->skb_list)) {
3435 struct sk_buff *old_skb = skb; /*check instead in aggregation condition that mem is shmem */
3436 max_nth_size = skb_headroom(skb);
3437 skb = ncm_wrap_ntb(port,skb,aggr_ctx);
3438 if (likely(skb)) {
3439 /*save first skb headroom for future aggregations limit*/
3440 if (max_nth_size > (ncb_len + 4 * MAX_IN_SKB_NUM)) {
3441 max_nth_size = (ncb_len + 4 * MAX_IN_SKB_NUM);
3442 }
3443 NTB_MAX_HEADER_SIZE(skb) = (old_skb==skb)? max_nth_size : 0;
3444 aggr_ctx->total_size = skb->len;
3445 skb_queue_tail(&aggr_ctx->skb_list, skb);
3446 }
3447 return skb;
3448 }
3449
3450 first_skb = skb_peek(&aggr_ctx->skb_list);
3451 prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
3452 max_nth_size = NTB_MAX_HEADER_SIZE(first_skb);
3453
3454 BUG_ON(!first_skb);
3455 BUG_ON(!prev_skb);
3456 BUG_ON(first_skb == prev_skb && 1!=skb_queue_len(&aggr_ctx->skb_list));
3457 BUG_ON(!aggr_ctx->total_size);
3458
3459 padding = (unsigned)skb->data - (unsigned)prev_skb->tail;
3460 aggregate_length = aggr_ctx->total_size + skb->len + padding;
3461 skb_qlen = skb_queue_len(&aggr_ctx->skb_list);
3462
3463 /* ncm aggregation condition */
3464 if (aggregate_length >= max_size ||
3465 (max_nth_size < (ncb_len + ndp_entry_pointer_len * (skb_qlen + 1))) ||
3466 (mbim->ntb_max_datagrams != 0 && mbim->ntb_max_datagrams <= skb_qlen) ||
3467 skb_qlen >= MAX_DGRAMS_AGGREGATION ||
3468 skb_cloned(skb) ||
3469 padding > AGGR_MAX_PADDING ||
3470 padding < 0) {
3471 aggr_ctx->pending_skb = skb;
3472 return first_skb;
3473 }
3474
3475 /* update NTH */
3476 nth_header = (void *)first_skb->data;
3477 /* (d)wBlockLength */
3478 put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->tail - first_skb->data);
3479
3480 /* update NDP */
3481 ndp_header = (void *)nth_header + opts->nth_size + ndp_pad;
3482 /* wLength */
3483 put_unaligned_le16(opts->ndp_size + ndp_entry_pointer_len * (skb_qlen + 2), &ndp_header->wLength);
3484
3485 /* skip previous packets ndp entries*/
3486 tmp = (void *)ndp_header + opts->ndp_size + ndp_entry_pointer_len * skb_qlen;
3487 /* (d)wDatagramIndex[i] */
3488 put_mbim(&tmp, opts->dgram_item_len, skb->data - first_skb->data);
3489 /* (d)wDatagramLength[i] */
3490 put_mbim(&tmp, opts->dgram_item_len, skb->len);
3491 /* (d)wDatagramIndex[N] and (d)wDatagramLength[N] already zeroed */
3492
3493 skb_queue_tail(&aggr_ctx->skb_list, skb);
3494 BUG_ON(skb->tail - first_skb->data != aggr_ctx->total_size + skb->len + padding);
3495 aggr_ctx->total_size = skb->tail - first_skb->data;
3496 //aggr_ctx->total_padding += padding;
3497 return first_skb;
3498}
3499#endif
3500#endif
3501
3502static int ncm_unwrap_ntb(struct gether *port,
3503 struct sk_buff *skb,
3504 struct sk_buff_head *list)
3505{
3506 struct f_mbim *mbim = func_to_mbim(&port->func);
3507 __le16 *tmp = (void *) skb->data;
3508 unsigned index, index2;
3509 unsigned dg_len, dg_len2;
3510 unsigned ndp_len;
3511 struct sk_buff *skb2;
3512 int ret = -EINVAL;
3513 unsigned max_size = le32_to_cpu(ntb_parameters.dwNtbOutMaxSize);
3514 struct ndp_parser_opts *opts = mbim->parser_opts;
3515 unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
3516 int dgram_counter;
3517
3518 /* dwSignature */
3519 if (get_unaligned_le32(tmp) != opts->nth_sign) {
3520 pr_err("Wrong NTH SIGN, skblen %d\n",
3521 skb->len);
3522 print_hex_dump(KERN_INFO, "HEAD:\n", DUMP_PREFIX_ADDRESS, 32, 1,
3523 skb->data, 32, false);
3524
3525 goto err;
3526 }
3527 tmp += 2;
3528 /* wHeaderLength */
3529 if (get_unaligned_le16(tmp++) != opts->nth_size) {
3530 pr_err("Wrong NTB headersize\n");
3531 goto err;
3532 }
3533 tmp++; /* skip wSequence */
3534
3535 /* (d)wBlockLength */
3536 if (get_mbim(&tmp, opts->block_length) > max_size) {
3537 pr_err("OUT size exceeded\n");
3538 goto err;
3539 }
3540
3541 index = get_mbim(&tmp, opts->fp_index);
3542 /* NCM 3.2 */
3543 if (((index % 4) != 0) && (index < opts->nth_size)) {
3544 pr_err("Bad index: %x\n",
3545 index);
3546 goto err;
3547 }
3548
3549 /* walk through NDP */
3550 tmp = ((void *)skb->data) + index;
3551 if (get_unaligned_le32(tmp) != opts->ndp_sign) {
3552 pr_err("Wrong NDP SIGN\n");
3553 goto err;
3554 }
3555 tmp += 2;
3556
3557 ndp_len = get_unaligned_le16(tmp++);
3558 /*
3559 * NCM 3.3.1
3560 * entry is 2 items
3561 * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
3562 * minimal: struct usb_cdc_ncm_ndpX + normal entry + zero entry
3563 */
3564 if ((ndp_len < opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2))
3565 || (ndp_len % opts->ndplen_align != 0)) {
3566 pr_err("Bad NDP length: %x\n", ndp_len);
3567 goto err;
3568 }
3569 tmp += opts->reserved1;
3570 tmp += opts->next_fp_index; /* skip reserved (d)wNextFpIndex */
3571 tmp += opts->reserved2;
3572
3573 ndp_len -= opts->ndp_size;
3574 index2 = get_mbim(&tmp, opts->dgram_item_len);
3575 dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
3576 dgram_counter = 0;
3577
3578 do {
3579 index = index2;
3580 dg_len = dg_len2;
3581 if (dg_len < 14 + crc_len) { /* ethernet header + crc */
3582 pr_err("Bad dgram length: %x\n",
3583 dg_len);
3584 goto err;
3585 }
3586 if (mbim->is_crc) {
3587 uint32_t crc, crc2;
3588
3589 crc = get_unaligned_le32(skb->data +
3590 index + dg_len - crc_len);
3591 crc2 = ~crc32_le(~0,
3592 skb->data + index,
3593 dg_len - crc_len);
3594 if (crc != crc2) {
3595 pr_err("Bad CRC\n");
3596 goto err;
3597 }
3598 }
3599
3600 index2 = get_mbim(&tmp, opts->dgram_item_len);
3601 dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
3602
3603 if (index2 == 0 || dg_len2 == 0) {
3604 skb2 = skb;
3605 } else {
3606 skb2 = skb_clone(skb, GFP_ATOMIC);
3607 if (skb2 == NULL)
3608 goto err;
3609 }
3610
3611 if (!skb_pull(skb2, index)) {
3612 ret = -EOVERFLOW;
3613 goto err;
3614 }
3615
3616 skb_trim(skb2, dg_len - crc_len);
3617 skb_queue_tail(list, skb2);
3618
3619 ndp_len -= 2 * (opts->dgram_item_len * 2);
3620
3621 dgram_counter++;
3622
3623 if (index2 == 0 || dg_len2 == 0)
3624 break;
3625 } while (ndp_len > 2 * (opts->dgram_item_len * 2)); /* zero entry */
3626
3627 VDBG(port->func.config->cdev,
3628 "Parsed NTB with %d frames\n", dgram_counter);
3629 return 0;
3630err:
3631 skb_queue_purge(list);
3632 dev_kfree_skb_any(skb);
3633 return ret;
3634}
3635#endif
3636
3637
3638
3639
3640/*-------------------------------------------------------------------------*/
3641
3642/*
3643 * Callbacks let us notify the host about connect/disconnect when the
3644 * net device is opened or closed.
3645 *
3646 * For testing, note that link states on this side include both opened
3647 * and closed variants of:
3648 *
3649 * - disconnected/unconfigured
3650 * - configured but inactive (data alt 0)
3651 * - configured and active (data alt 1)
3652 *
3653 * Each needs to be tested with unplug, rmmod, SET_CONFIGURATION, and
3654 * SET_INTERFACE (altsetting). Remember also that "configured" doesn't
3655 * imply the host is actually polling the notification endpoint, and
3656 * likewise that "active" doesn't imply it's actually using the data
3657 * endpoints for traffic.
3658 */
3659
3660static void __maybe_unused mbim_net_open(struct gether *geth)
3661{
3662 struct f_mbim *mbim = func_to_mbim(&geth->func);
3663
3664 spin_lock(&mbim->lock);
3665 mbim->is_open = true;
3666 mbim_notify(mbim);
3667 spin_unlock(&mbim->lock);
3668}
3669
3670static void __maybe_unused mbim_net_close(struct gether *geth)
3671{
3672 struct f_mbim *mbim = func_to_mbim(&geth->func);
3673
3674 spin_lock(&mbim->lock);
3675 mbim->is_open = false;
3676 mbim_notify(mbim);
3677 spin_unlock(&mbim->lock);
3678}
3679
3680/*-------------------------------------------------------------------------*/
3681
3682/* ethernet function driver setup/binding */
3683
3684static int
3685mbim_bind(struct usb_configuration *c, struct usb_function *f)
3686{
3687 struct usb_composite_dev *cdev = c->cdev;
3688 struct f_mbim *mbim = func_to_mbim(f);
3689 int status;
3690 struct usb_ep *ep;
3691
3692 /* allocate instance-specific interface IDs */
3693 status = usb_interface_id(c, f);
3694 if (status < 0)
3695 goto fail;
3696 mbim->ctrl_id = status;
3697 mbim_iad_desc.bFirstInterface = status;
3698
3699 ncm_control_intf.bInterfaceNumber = status;
3700 mbim_control_intf.bInterfaceNumber = status;
3701 ncm_mbim_control_intf.bInterfaceNumber = status;
3702 mbim_union_desc.bMasterInterface0 = status;
3703
3704 status = usb_interface_id(c, f);
3705 if (status < 0)
3706 goto fail;
3707 mbim->data_id = status;
3708
3709 ncm_data_nop_intf.bInterfaceNumber = status;
3710 ncm_data_intf.bInterfaceNumber = status;
3711 mbim_data_nop_intf.bInterfaceNumber = status;
3712 mbim_data_intf.bInterfaceNumber = status;
3713 mbim_union_desc.bSlaveInterface0 = status;
3714 ncm_mbim_data_intf.bInterfaceNumber = status;
3715
3716 status = -ENODEV;
3717
3718 /* allocate instance-specific endpoints */
3719 ep = usb_ep_autoconfig(cdev->gadget, &fs_mbim_in_desc);
3720 if (!ep)
3721 goto fail;
3722 mbim->port.in_ep = ep;
3723 ep->driver_data = cdev; /* claim */
3724
3725 ep = usb_ep_autoconfig(cdev->gadget, &fs_mbim_out_desc);
3726 if (!ep)
3727 goto fail;
3728 mbim->port.out_ep = ep;
3729 ep->driver_data = cdev; /* claim */
3730
3731 ep = usb_ep_autoconfig(cdev->gadget, &fs_mbim_notify_desc);
3732 if (!ep)
3733 goto fail;
3734 mbim->notify = ep;
3735 ep->driver_data = cdev; /* claim */
3736
3737 status = -ENOMEM;
3738
3739 /* allocate notification request and buffer */
3740 mbim->notify_req = mbim_alloc_req(ep, MBIM_STATUS_BYTECOUNT);
3741 if (!mbim->notify_req || !mbim->notify_req->buf)
3742 goto fail;
3743
3744 mbim->notify_req->context = mbim;
3745 mbim->notify_req->complete = mbim_notify_complete;
3746
3747 g_notify_req = mbim->notify_req;
3748 /*
3749 * support all relevant hardware speeds... we expect that when
3750 * hardware is dual speed, all bulk-capable endpoints work at
3751 * both speeds
3752 */
3753 hs_mbim_in_desc.bEndpointAddress =
3754 fs_mbim_in_desc.bEndpointAddress;
3755 hs_mbim_out_desc.bEndpointAddress =
3756 fs_mbim_out_desc.bEndpointAddress;
3757 hs_mbim_notify_desc.bEndpointAddress =
3758 fs_mbim_notify_desc.bEndpointAddress;
3759
3760 ss_mbim_in_desc.bEndpointAddress =
3761 fs_mbim_in_desc.bEndpointAddress;
3762 ss_mbim_out_desc.bEndpointAddress =
3763 fs_mbim_out_desc.bEndpointAddress;
3764 ss_mbim_notify_desc.bEndpointAddress =
3765 fs_mbim_notify_desc.bEndpointAddress;
3766
3767 /* copy descriptors, and track endpoint copies */
3768 switch (mbim->mode) {
3769 case MODE_NCM:
3770 status = usb_assign_descriptors(f, ncm_fs_function, ncm_hs_function,
3771 ncm_ss_function, ncm_ss_function);
3772 break;
3773 case MODE_MBIM:
3774 status = usb_assign_descriptors(f, mbim_fs_function, mbim_hs_function,
3775 mbim_ss_function, mbim_ss_function);
3776 break;
3777 case MODE_NCM_MBIM:
3778 status = usb_assign_descriptors(f, ncm_mbim_fs_function, ncm_mbim_hs_function,
3779 ncm_mbim_ss_function, ncm_mbim_ss_function);
3780 break;
3781 default:
3782 BUG();
3783 }
3784#if 0
3785 /*
3786 * support all relevant hardware speeds... we expect that when
3787 * hardware is dual speed, all bulk-capable endpoints work at
3788 * both speeds
3789 */
3790 if (gadget_is_dualspeed(c->cdev->gadget)) {
3791 hs_mbim_in_desc.bEndpointAddress =
3792 fs_mbim_in_desc.bEndpointAddress;
3793 hs_mbim_out_desc.bEndpointAddress =
3794 fs_mbim_out_desc.bEndpointAddress;
3795 hs_mbim_notify_desc.bEndpointAddress =
3796 fs_mbim_notify_desc.bEndpointAddress;
3797
3798 /* copy descriptors, and track endpoint copies */
3799 switch (mbim->mode) {
3800 case MODE_NCM:
3801 f->hs_descriptors = usb_copy_descriptors(ncm_hs_function);
3802 break;
3803 case MODE_MBIM:
3804 f->hs_descriptors = usb_copy_descriptors(mbim_hs_function);
3805 break;
3806 case MODE_NCM_MBIM:
3807 f->hs_descriptors = usb_copy_descriptors(ncm_mbim_hs_function);
3808 break;
3809 default:
3810 BUG();
3811 }
3812 if (!f->hs_descriptors)
3813 goto fail;
3814 }
3815#endif
3816 /*
3817 * NOTE: all that is done without knowing or caring about
3818 * the network link ... which is unavailable to this code
3819 * until we're activated via set_alt().
3820 */
3821
3822 if (mbim->mode == MODE_NCM) {
3823 mbim->port.open = mbim_net_open;
3824 mbim->port.close = mbim_net_close;
3825 } else {
3826 /* Ruslan: originally was commented out in this driver*/
3827 mbim->port.open = NULL;
3828 mbim->port.close = NULL;
3829 }
3830
3831 pr_info("%s speed IN/%s OUT/%s NOTIFY/%s\n",
3832 gadget_is_superspeed(c->cdev->gadget) ? "super" :
3833 gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
3834 mbim->port.in_ep->name, mbim->port.out_ep->name,
3835 mbim->notify->name);
3836#ifdef CONFIG_ASR_TOE
3837 if (mbim->mode == MODE_NCM) {
3838 mbim->port.ueth_type = UETHER_NCM;
3839 } else {
3840 mbim->port.ueth_type = UETHER_MBIM;
3841 }
3842#endif
3843
3844 return 0;
3845
3846fail:
3847 pr_err("%s failed to bind, err %d\n", f->name, status);
3848
3849 usb_free_all_descriptors(f);
3850
3851 if (mbim->notify_req) {
3852 kfree(mbim->notify_req->buf);
3853 usb_ep_free_request(mbim->notify, mbim->notify_req);
3854 }
3855
3856 /* we might as well release our claims on endpoints */
3857 if (mbim->notify)
3858 mbim->notify->driver_data = NULL;
3859 if (mbim->port.out_ep)
3860 mbim->port.out_ep->driver_data = NULL;
3861 if (mbim->port.in_ep)
3862 mbim->port.in_ep->driver_data = NULL;
3863
3864 return status;
3865}
3866
3867static void
3868mbim_unbind(struct usb_configuration *c, struct usb_function *f)
3869{
3870 struct f_mbim *mbim = func_to_mbim(f);
3871
3872#ifdef CONFIG_ASR_TOE
3873 mbim->port.ueth_type = UETHER_UNKNOWN;
3874#endif
3875 usb_free_all_descriptors(f);
3876
3877 kfree(mbim->notify_req->buf);
3878 g_notify_req = NULL;
3879 usb_ep_free_request(mbim->notify, mbim->notify_req);
3880 #if defined(CONFIG_CPU_ASR18XX) && defined(CONFIG_USB_MVC2)
3881 if (gadget_current_is_dualspeed(c->cdev->gadget))
3882 mbim_string_defs[0].id = 0;
3883 #endif
3884}
3885
3886/**
3887 * mbim_bind_config - add MBIM link to a configuration
3888 * @c: the configuration to support the network link
3889 * @ethaddr: a buffer in which the ethernet address of the host side
3890 * side of the link was recorded
3891 * Context: single threaded during gadget setup
3892 *
3893 * Returns zero on success, else negative errno.
3894 *
3895 * Caller must have called @gether_setup(). Caller is also responsible
3896 * for calling @gether_cleanup() before module unload.
3897 */
3898int mbim_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN], struct eth_dev *dev, enum ncm_mbim_mode mode)
3899{
3900 struct f_mbim *mbim;
3901 int status, i;
3902
3903#ifdef CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS
3904 pr_info("CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS defined\n");
3905#endif
3906 if (!can_support_ecm(c->cdev->gadget) || !ethaddr)
3907 return -EINVAL;
3908
3909 /* maybe allocate device-global string IDs */
3910 if (mbim_string_defs[0].id == 0) {
3911
3912 /* control interface label */
3913 status = usb_string_id(c->cdev);
3914 if (status < 0)
3915 return status;
3916 mbim_string_defs[STRING_CTRL_IDX].id = status;
3917 mbim_control_intf.iInterface = status;
3918
3919 /* data interface label */
3920 status = usb_string_id(c->cdev);
3921 if (status < 0)
3922 return status;
3923 mbim_string_defs[STRING_DATA_IDX].id = status;
3924 mbim_data_nop_intf.iInterface = status;
3925 mbim_data_intf.iInterface = status;
3926
3927 /* MAC address */
3928 status = usb_string_id(c->cdev);
3929 if (status < 0)
3930 return status;
3931 mbim_string_defs[STRING_MAC_IDX].id = status;
3932 ether_desc.iMACAddress = status;
3933
3934 /* IAD */
3935 status = usb_string_id(c->cdev);
3936 if (status < 0)
3937 return status;
3938 mbim_string_defs[STRING_IAD_IDX].id = status;
3939 mbim_iad_desc.iFunction = status;
3940 }
3941
3942 /* allocate and initialize one new instance */
3943 mbim = mbim_ports[0].port;
3944 if (!mbim) {
3945 pr_info("mbim struct not allocated\n");
3946 return -ENOMEM;
3947 }
3948
3949 /* export host's Ethernet address in CDC format */
3950 snprintf(mbim->dev_addr, sizeof mbim->dev_addr,
3951 "%02X%02X%02X%02X%02X%02X\n",
3952 ethaddr[0], ethaddr[1], ethaddr[2],
3953 ethaddr[3], ethaddr[4], ethaddr[5]);
3954 mbim_string_defs[1].s = mbim->dev_addr;
3955
3956 mbim->host_addr[0] = 0x00;
3957 mbim->host_addr[1] = 0xFF;
3958 mbim->host_addr[2] = 0xFF;
3959 mbim->host_addr[3] = 0xFF;
3960 mbim->host_addr[4] = 0xFF;
3961 mbim->host_addr[5] = 0x00;
3962
3963 spin_lock_init(&mbim->lock);
3964 mbim_reset_values(mbim);
3965 mbim->port.is_fixed = true;
3966 mbim->port.ioport = dev;
3967 mbim->mode = mode;
3968
3969 switch (mbim->mode) {
3970 case MODE_NCM:
3971 mbim->port.func.name = "ncm";
3972 break;
3973 case MODE_MBIM:
3974 mbim->port.func.name = "mbim";
3975 break;
3976 case MODE_NCM_MBIM:
3977 mbim->port.func.name = "ncm_mbim";
3978 break;
3979 default:
3980 BUG();
3981 }
3982
3983 mbim->port.func.strings = mbim_strings;
3984 /* descriptors are per-instance copies */
3985 mbim->port.func.bind = mbim_bind;
3986 mbim->port.func.unbind = mbim_unbind;
3987 mbim->port.func.set_alt = mbim_set_alt;
3988 mbim->port.func.get_alt = mbim_get_alt;
3989 mbim->port.func.setup = mbim_setup;
3990 mbim->port.func.disable = mbim_disable;
3991
3992 INIT_LIST_HEAD(&mbim->cpkt_req_q);
3993 INIT_LIST_HEAD(&mbim->cpkt_resp_q);
3994
3995 if (mbim->mode == MODE_NCM || mbim->mode == MODE_NCM_MBIM) {
3996#ifdef CONFIG_USB_G_NCM_MULT_PKT_SUPPORT
3997 mbim->port.wrap = ncm_wrap_ntb_multipkt;
3998#else
3999 mbim->port.wrap = ncm_wrap_ntb;
4000#endif
4001 mbim->port.unwrap = ncm_unwrap_ntb;
4002 mbim->port.unwrap_fixup = NULL;
4003 }
4004 if (mbim->mode == MODE_MBIM) {
4005#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
4006 mbim->port.wrap = mbim_wrap_ntb_multipkt;
4007#ifdef CONFIG_USBNET_USE_SG
4008 mbim->port.is_sg_mode = 1;
4009#endif
4010#else
4011 mbim->port.wrap = mbim_wrap_ntb;
4012#endif
4013 mbim->port.unwrap = mbim_unwrap_ntb;
4014#ifndef CONFIG_ASR_TOE
4015 mbim->port.unwrap_fixup = mbim_process_dgram;
4016#endif
4017 }
4018
4019 for (i = (ETH_ALEN-2); i < (ETH_ALEN-1); i++) {
4020 mbim->host_addr[i] = i;
4021 }
4022
4023 status = usb_add_function(c, &mbim->port.func);
4024 if (status) {
4025 mbim_string_defs[1].s = NULL;
4026 kfree(mbim);
4027 }
4028 return status;
4029}
4030
4031/* ------------ MBIM DRIVER File Operations API for USER SPACE ------------ */
4032
4033#define MBIM_BULK_BUFFER_SIZE 4096
4034#define MBIM_IOCTL_MAGIC 'o'
4035#define MBIM_GET_NTB_SIZE _IOR(MBIM_IOCTL_MAGIC, 2, u32)
4036#define MBIM_GET_DATAGRAM_COUNT _IOR(MBIM_IOCTL_MAGIC, 3, u16)
4037
4038/* temporary variable used between mbim_open() and mbim_gadget_bind() */
4039static struct f_mbim *_mbim_dev;
4040static unsigned int nr_mbim_ports;
4041static int mbim_inited = 0;
4042static ssize_t
4043mbim_read(struct file *fp, char __user *buf, size_t count, loff_t *pos)
4044{
4045 struct f_mbim *dev = fp->private_data;
4046 struct ctrl_pkt *cpkt = NULL;
4047 int ret = 0;
4048
4049 pr_debug("(%d)\n", count);
4050
4051 if (!dev) {
4052 pr_err("Received NULL mbim pointer\n");
4053 return -ENODEV;
4054 }
4055
4056 if (dev->mode != MODE_MBIM && dev->mode != MODE_NCM_MBIM) {
4057 pr_err("NOT mbim mode\n");
4058 return -ENODEV;
4059 }
4060
4061 if (count > MBIM_BULK_BUFFER_SIZE) {
4062 pr_err("Buffer size is too big %d, should be at most %d\n",
4063 count, MBIM_BULK_BUFFER_SIZE);
4064 return -EINVAL;
4065 }
4066
4067 if (mbim_lock(&dev->read_excl)) {
4068 pr_err("Previous reading is not finished yet\n");
4069 return -EBUSY;
4070 }
4071
4072 if (atomic_read(&dev->error)) {
4073 mbim_unlock(&dev->read_excl);
4074 return -EIO;
4075 }
4076
4077 while (list_empty(&dev->cpkt_req_q)) {
4078 pr_debug("Requests list is empty. Wait.\n");
4079 ret = wait_event_interruptible(dev->read_wq,
4080 !list_empty(&dev->cpkt_req_q));
4081 if (ret < 0) {
4082 pr_debug("Waiting interrupted\n");
4083 mbim_unlock(&dev->read_excl);
4084 return 0;
4085 }
4086 pr_debug("Received request packet\n");
4087 }
4088
4089 cpkt = list_first_entry(&dev->cpkt_req_q, struct ctrl_pkt,
4090 list);
4091 if (cpkt->len > count) {
4092 mbim_unlock(&dev->read_excl);
4093 pr_err("cpkt size too big:%d > buf size:%d\n",
4094 cpkt->len, count);
4095 return -ENOMEM;
4096 }
4097
4098 pr_debug("cpkt size:%d trans_id:%d\n", cpkt->len, cpkt->transaction_id);
4099
4100 list_del(&cpkt->list);
4101 mbim_unlock(&dev->read_excl);
4102
4103 ret = copy_to_user(buf, cpkt->buf, cpkt->len);
4104 if (ret) {
4105 pr_err("copy_to_user failed: err %d\n", ret);
4106 ret = 0;
4107 } else {
4108 pr_debug("copied %d bytes to user\n", cpkt->len);
4109 ret = cpkt->len;
4110 }
4111
4112 mbim_free_ctrl_pkt(cpkt);
4113
4114 return ret;
4115}
4116
4117static ssize_t
4118mbim_write(struct file *fp, const char __user *buf, size_t count, loff_t *pos)
4119{
4120 struct f_mbim *dev = fp->private_data;
4121 struct ctrl_pkt *cpkt = NULL;
4122 int ret = 0;
4123 struct usb_cdc_mbim_ctrl_msg *ctrl_msg = (struct usb_cdc_mbim_ctrl_msg *)buf;
4124
4125 pr_debug("(%d)\n", count);
4126
4127 if (!dev) {
4128 pr_err("Received NULL mbim pointer\n");
4129 return -ENODEV;
4130 }
4131
4132 if (dev->mode != MODE_MBIM && dev->mode != MODE_NCM_MBIM) {
4133 pr_err("NOT mbim mode\n");
4134 return -ENODEV;
4135 }
4136
4137 if (!count) {
4138 pr_err("zero length ctrl pkt\n");
4139 return -ENODEV;
4140 }
4141
4142 if (count > MAX_CTRL_PKT_SIZE) {
4143 pr_err("given pkt size too big:%d > max_pkt_size:%d\n",
4144 count, MAX_CTRL_PKT_SIZE);
4145 return -ENOMEM;
4146 }
4147
4148 if (mbim_lock(&dev->write_excl)) {
4149 pr_err("Previous writing not finished yet\n");
4150 return -EBUSY;
4151 }
4152
4153 if (!atomic_read(&dev->online)) {
4154 pr_info("MBIM not connected\n");
4155 mbim_unlock(&dev->write_excl);
4156 return -EPIPE;
4157 }
4158
4159 /* Hold up to 64 responses in the queue */
4160 if (atomic_read(&dev->response_available) > MBIM_MAX_RESP_Q) {
4161 pr_err("Too much responses pending!\n");
4162 mbim_unlock(&dev->write_excl);
4163 return -EBUSY;
4164 }
4165
4166 cpkt = mbim_alloc_ctrl_pkt(count, GFP_KERNEL);
4167 if (!cpkt) {
4168 pr_err("failed to allocate ctrl pkt\n");
4169 mbim_unlock(&dev->write_excl);
4170 return -ENOMEM;
4171 }
4172
4173 cpkt->transaction_id = ctrl_msg->dwTransactionId;
4174 ret = copy_from_user(cpkt->buf, buf, count);
4175 if (ret) {
4176 pr_err("copy_from_user failed err:%d\n", ret);
4177 mbim_free_ctrl_pkt(cpkt);
4178 mbim_unlock(&dev->write_excl);
4179 return 0;
4180 }
4181
4182 fmbim_send_cpkt_response(dev, cpkt);
4183
4184 mbim_unlock(&dev->write_excl);
4185
4186 return count;
4187}
4188
4189static int mbim_open(struct inode *ip, struct file *fp)
4190{
4191 if (!_mbim_dev) {
4192 pr_err("mbim_dev not created yet\n");
4193 return -ENODEV;
4194 }
4195
4196 if (_mbim_dev->mode != MODE_MBIM && _mbim_dev->mode != MODE_NCM_MBIM) {
4197 pr_err("NOT mbim mode\n");
4198 return -ENODEV;
4199 }
4200
4201 if (mbim_lock(&_mbim_dev->open_excl)) {
4202 pr_err("Already opened\n");
4203 return -EBUSY;
4204 }
4205
4206 if (!atomic_read(&_mbim_dev->online))
4207 pr_info("MBIM not connected\n");
4208
4209 pr_debug("Lock mbim_dev->open_excl for open\n");
4210
4211 fp->private_data = _mbim_dev;
4212
4213 atomic_set(&_mbim_dev->error, 0);
4214
4215 // TODO - Check if needed
4216 spin_lock(&_mbim_dev->lock);
4217 _mbim_dev->is_open = true;
4218 if (atomic_read(&_mbim_dev->online))
4219 mbim_notify(_mbim_dev);
4220 spin_unlock(&_mbim_dev->lock);
4221
4222 pr_debug("Exit, mbim file opened\n");
4223
4224 return 0;
4225}
4226
4227static int mbim_release(struct inode *ip, struct file *fp)
4228{
4229 struct f_mbim *mbim = fp->private_data;
4230 u8 max_loop = 10;
4231
4232 if (mbim->mode != MODE_MBIM && mbim->mode != MODE_NCM_MBIM) {
4233 pr_err("NOT mbim mode\n");
4234 return -ENODEV;
4235 }
4236
4237 spin_lock(&mbim->lock);
4238 mbim->is_open = false;
4239 spin_unlock(&mbim->lock);
4240
4241 do {
4242 spin_lock(&mbim->lock);
4243 if (atomic_read(&mbim->response_available) == 0 &&
4244 mbim->notify_req != NULL) {
4245 spin_unlock(&mbim->lock);
4246 break;
4247 }
4248
4249 pr_err("in progress, waiting for all notifications to complete,"
4250 "response_available = %d, notify_req = %p\n",
4251 atomic_read(&mbim->response_available), mbim->notify_req);
4252 spin_unlock(&mbim->lock);
4253
4254 if (--max_loop == 0) {
4255 pr_err("Closing the file handler without notifying the host\n");
4256 mbim_unlock(&_mbim_dev->open_excl);
4257 return 0;
4258 }
4259
4260 msleep(2000);
4261 } while (1);
4262
4263 BUG_ON(!mbim->notify_req);
4264 BUG_ON(atomic_read(&mbim->response_available));
4265
4266 /* add spinlock to protect the mbim_notify */
4267 spin_lock(&mbim->lock);
4268 if (atomic_read(&_mbim_dev->online))
4269 mbim_notify(mbim);
4270 spin_unlock(&mbim->lock);
4271
4272 mbim_unlock(&_mbim_dev->open_excl);
4273
4274 return 0;
4275}
4276
4277static long mbim_ioctl(struct file *fp, unsigned cmd, unsigned long arg)
4278{
4279 struct f_mbim *mbim = fp->private_data;
4280 int ret = 0;
4281
4282 pr_debug("Received command %d\n", cmd);
4283
4284 if (mbim->mode != MODE_MBIM && mbim->mode != MODE_NCM_MBIM) {
4285 pr_err("NOT mbim mode\n");
4286 return -ENODEV;
4287 }
4288
4289 if (mbim_lock(&mbim->ioctl_excl))
4290 return -EBUSY;
4291
4292 switch (cmd) {
4293 case MBIM_GET_NTB_SIZE:
4294 ret = copy_to_user((void __user *)arg,
4295 &mbim->ntb_input_size, sizeof(mbim->ntb_input_size));
4296 if (ret) {
4297 pr_err("copying to user space failed\n");
4298 ret = -EFAULT;
4299 }
4300 pr_info("Sent NTB size %d\n", mbim->ntb_input_size);
4301 break;
4302 case MBIM_GET_DATAGRAM_COUNT:
4303 ret = copy_to_user((void __user *)arg,
4304 &mbim->ntb_max_datagrams,
4305 sizeof(mbim->ntb_max_datagrams));
4306 if (ret) {
4307 pr_err("copying to user space failed\n");
4308 ret = -EFAULT;
4309 }
4310 pr_info("Sent NTB datagrams count %d\n",
4311 mbim->ntb_max_datagrams);
4312 break;
4313 default:
4314 pr_err("wrong parameter\n");
4315 ret = -EINVAL;
4316 }
4317
4318 mbim_unlock(&mbim->ioctl_excl);
4319
4320 return ret;
4321}
4322
4323/* file operations for MBIM device /dev/mbim */
4324static const struct file_operations mbim_fops = {
4325 .owner = THIS_MODULE,
4326 .open = mbim_open,
4327 .release = mbim_release,
4328 .read = mbim_read,
4329 .write = mbim_write,
4330 .unlocked_ioctl = mbim_ioctl,
4331};
4332
4333static struct miscdevice mbim_device = {
4334 .minor = MISC_DYNAMIC_MINOR,
4335 .name = "mbim",
4336 .fops = &mbim_fops,
4337};
4338
4339static int mbim_init(int instances)
4340{
4341 int i;
4342 struct f_mbim *dev = NULL;
4343 int ret;
4344
4345 if (mbim_inited)
4346 return 0;
4347
4348 pr_info("initialize %d instances\n", instances);
4349
4350 if (instances > NR_MBIM_PORTS) {
4351 pr_err("Max-%d instances supported\n", NR_MBIM_PORTS);
4352 return -EINVAL;
4353 }
4354
4355 for (i = 0; i < instances; i++) {
4356 dev = kzalloc(sizeof(struct f_mbim), GFP_KERNEL);
4357 if (!dev) {
4358 pr_err("Failed to allocate mbim dev\n");
4359 ret = -ENOMEM;
4360 goto fail_probe;
4361 }
4362
4363 dev->port_num = i;
4364 spin_lock_init(&dev->lock);
4365 INIT_LIST_HEAD(&dev->cpkt_req_q);
4366 INIT_LIST_HEAD(&dev->cpkt_resp_q);
4367
4368 mbim_ports[i].port = dev;
4369 mbim_ports[i].port_num = i;
4370
4371 dev->mode = MODE_MBIM;
4372 dev->state = STATE_MBIM;
4373
4374 init_waitqueue_head(&dev->read_wq);
4375 init_waitqueue_head(&dev->write_wq);
4376 init_waitqueue_head(&dev->open_wq);
4377 atomic_set(&dev->open_excl, 0);
4378 atomic_set(&dev->ioctl_excl, 0);
4379 atomic_set(&dev->read_excl, 0);
4380 atomic_set(&dev->write_excl, 0);
4381 atomic_set(&dev->response_available, 0);
4382
4383 nr_mbim_ports++;
4384
4385 }
4386
4387 _mbim_dev = dev;
4388 ret = misc_register(&mbim_device);
4389 if (ret) {
4390 pr_err("mbim driver failed to register\n");
4391 goto fail_probe;
4392 }
4393
4394 pr_info("Initialized %d ports\n", nr_mbim_ports);
4395 mbim_inited = 1;
4396 return ret;
4397
4398fail_probe:
4399 pr_err("Failed\n");
4400 for (i = 0; i < nr_mbim_ports; i++) {
4401 kfree(mbim_ports[i].port);
4402 mbim_ports[i].port = NULL;
4403 }
4404
4405 return ret;
4406}
4407
4408static void __maybe_unused fmbim_cleanup(void)
4409{
4410#if defined (ALLOW_MBIM_CLEANUP)
4411 int i = 0;
4412
4413 for (i = 0; i < nr_mbim_ports; i++) {
4414 /* to avoid memory leak */
4415 mbim_clear_queues(mbim_ports[i].port);
4416 kfree(mbim_ports[i].port);
4417 mbim_ports[i].port = NULL;
4418 }
4419 nr_mbim_ports = 0;
4420
4421 misc_deregister(&mbim_device);
4422 mbim_device.minor = MISC_DYNAMIC_MINOR;
4423 _mbim_dev = NULL;
4424#endif
4425}