ASR_BASE
Change-Id: Icf3719cc0afe3eeb3edc7fa80a2eb5199ca9dda1
diff --git a/marvell/linux/drivers/usb/gadget/function/f_mbim.c b/marvell/linux/drivers/usb/gadget/function/f_mbim.c
new file mode 100644
index 0000000..526d586
--- /dev/null
+++ b/marvell/linux/drivers/usb/gadget/function/f_mbim.c
@@ -0,0 +1,4425 @@
+/*
+ * f_mbim.c -- USB CDC Network (MBIM) link function driver
+ *
+ * Copyright (C) 2014 Marvell
+ * Contact: Yaniv Yizhak <yyaniv@marvell.com>
+ *
+ * The driver borrows from f_ecm.c which is:
+ *
+ * Copyright (C) 2003-2005,2008 David Brownell
+ * Copyright (C) 2008 Nokia Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/etherdevice.h>
+#include <linux/crc32.h>
+#include <linux/usb/cdc.h>
+#include <linux/if_vlan.h>
+#include <linux/if_arp.h>
+#include <net/arp.h>
+#include <linux/spinlock.h>
+#include "u_ether.h"
+
+#ifdef CONFIG_ASR_TOE
+#define CONFIG_NCM_INPUT_AS_RNDIS 1
+#define CONFIG_MBIM_INPUT_AS_RNDIS 1
+#include <linux/toe.h>
+#if (defined CONFIG_NCM_INPUT_AS_RNDIS) || (defined CONFIG_MBIM_INPUT_AS_RNDIS)
+#include "rndis.h"
+#endif
+#endif
+
+
+static struct sk_buff *ncm_wrap_ntb(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx);
+static struct sk_buff *mbim_wrap_ntb(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx);
+#ifdef CONFIG_USB_G_NCM_MULT_PKT_SUPPORT
+static struct sk_buff *ncm_wrap_ntb_multipkt(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx);
+#endif
+#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
+static struct sk_buff *mbim_wrap_ntb_multipkt(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx);
+#endif
+static int ncm_unwrap_ntb(struct gether *port, struct sk_buff *skb,
+ struct sk_buff_head *list);
+static int mbim_unwrap_ntb(struct gether *port, struct sk_buff *skb,
+ struct sk_buff_head *list);
+#ifndef CONFIG_ASR_TOE
+static struct sk_buff *mbim_process_dgram(struct gether *port,
+ struct sk_buff *skb);
+#endif
+static void __maybe_unused mbim_net_open(struct gether *geth);
+static void __maybe_unused mbim_net_close(struct gether *geth);
+
+/*
+ * This function is a "Mobile Broadband Interface Model" (MBIM) link.
+ * MBIM is intended to be used with high-speed network attachments.
+ *
+ * Note that MBIM requires the use of "alternate settings" for its data
+ * interface. This means that the set_alt() method has real work to do,
+ * and also means that a get_alt() method is required.
+ */
+
+/* to trigger crc/non-crc NDP (NCM Datagram Pointer) signature */
+#define NCM_NDP_HDR_CRC_MASK 0x01000000
+#define NCM_NDP_HDR_CRC 0x01000000
+#define NCM_NDP_HDR_NOCRC 0x00000000
+
+/*
+ * Use wMaxPacketSize big enough to fit CDC_NOTIFY_SPEED_CHANGE in one
+ * packet, to simplify cancellation; and a big transfer interval, to
+ * waste less bandwidth.
+ */
+#define LOG2_STATUS_INTERVAL_MSEC 5 /* 1 << 5 == 32 msec */
+#define NCM_STATUS_BYTECOUNT 16 /* 8 byte header + data */
+#define MBIM_STATUS_BYTECOUNT (NCM_STATUS_BYTECOUNT)
+
+enum mbim_notify_state {
+ MBIM_NOTIFY_NONE, /* don't notify */
+ MBIM_NOTIFY_CONNECT, /* issue CONNECT next */
+ MBIM_NOTIFY_SPEED, /* issue SPEED_CHANGE next */
+};
+
+enum ncm_mbim_state {
+ STATE_NCM,
+ STATE_MBIM,
+};
+
+enum ncm_mbim_mode {
+ MODE_NCM,
+ MODE_MBIM,
+ MODE_NCM_MBIM,
+ MODE_UNKNOWN,
+};
+
+/* Control packet definition */
+#define MAX_CTRL_PKT_SIZE (4096)
+struct ctrl_pkt {
+ void *buf;
+ int len;
+ __le32 transaction_id;
+ struct list_head list;
+};
+
+struct f_mbim {
+ struct gether port;
+ char dev_addr[14];
+ char host_addr[ETH_ALEN];
+ struct usb_ep *notify;
+ struct usb_endpoint_descriptor *notify_desc;
+ struct usb_request *notify_req;
+ atomic_t notify_count;
+ u8 notify_state;
+ bool is_open;
+ atomic_t response_available;
+ atomic_t online;
+ atomic_t open_excl;
+ atomic_t ioctl_excl;
+ atomic_t read_excl;
+ atomic_t write_excl;
+ wait_queue_head_t read_wq;
+ wait_queue_head_t write_wq;
+ wait_queue_head_t open_wq;
+ u8 port_num;
+ u8 ctrl_id, data_id;
+ struct ndp_parser_opts *parser_opts;
+ bool is_crc;
+ spinlock_t lock;
+ struct list_head cpkt_req_q;
+ struct list_head cpkt_resp_q;
+ u32 ntb_input_size;
+ u16 ntb_max_datagrams;
+ atomic_t error;
+
+ enum ncm_mbim_mode mode;
+ enum ncm_mbim_state state;
+};
+
+/* Number of MBIM ports */
+#define NR_MBIM_PORTS 1
+static struct mbim_ports {
+ struct f_mbim *port;
+ unsigned port_num;
+} mbim_ports[NR_MBIM_PORTS];
+
+static struct usb_request *g_notify_req;
+
+static inline struct f_mbim *func_to_mbim(struct usb_function *f)
+{
+ return container_of(f, struct f_mbim, port.func);
+}
+
+/* peak (theoretical) bulk transfer rate in bits-per-second */
+static inline unsigned mbim_bitrate(struct usb_gadget *g)
+{
+ if (gadget_is_dualspeed(g) && g->speed == USB_SPEED_HIGH)
+ return (13 * 512 * 8 * 1000 * 8);
+ else
+ return (19 * 64 * 1 * 1000 * 8);
+}
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * skbs of size less than that will not be aligned
+ * to MBIM's dwNtbInMaxSize to save bus bandwidth
+ */
+#define MAX_TX_NONFIXED (512 * 3)
+
+/*
+ * Prevent memory waste here.
+ * NTB size 0x4000 is 16kB, but during skb allocation
+ * it will be reserved some additional memory in headroom,
+ * so final chunk to allocate is a little bit more than 16k.
+ * This leads to waste of memory because SLAB allocator
+ * will use 32kB buffers.
+ * So reduce maximum NTB size by 0x200 bytes that
+ * is quite enough for preventing usage of 32k buffers
+ * and performance doesn't have noticeable degradation
+ */
+
+#define MAX_IN_SKB_NUM (12) /* don't use too much headroom */
+#ifdef CONFIG_ASR_TOE
+#define NTB_IN_SIZE (16384)
+/* min value: ncm header(24+40*4) + 1514 + padding = 1700
+* max value: 2048 - 64(bm header) - sizeof(struct skb) = 1784
+*/
+#define NTB_OUT_SIZE (1728) /* 27 * 64 */
+#define MAX_DGRAMS_AGGREGATION (1)
+#else
+#ifndef CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS
+#define NTB_IN_SIZE (1600 * 10) /* 1500+14+64+alignment+extra 8bytes*/
+#define NTB_OUT_SIZE (2048)
+#define MAX_DGRAMS_AGGREGATION (10)
+#else
+#define NTB_IN_SIZE (16000) /* 1500+14+64+alignment+extra 8bytes*/
+#define NTB_OUT_SIZE (0x2000 - 256 - 2)
+#define MAX_DGRAMS_AGGREGATION (40)
+#endif
+#endif
+
+#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
+#ifdef CONFIG_USBNET_USE_SG
+#define MBIM_MAX_DGRAMS_AGGREGATION (8)
+#else
+#define MBIM_MAX_DGRAMS_AGGREGATION (10)
+#endif
+#endif
+
+#define NDP_IN_DIVISOR (0x4)
+
+
+#define FORMATS_SUPPORTED (USB_CDC_NCM_NTB16_SUPPORTED | USB_CDC_NCM_NTB32_SUPPORTED)
+
+/* NCM Transfer Block parameters (also used for GET_NTB_PARAMETERS request)*/
+static struct usb_cdc_ncm_ntb_parameters ntb_parameters = {
+ .wLength = sizeof ntb_parameters,
+ .bmNtbFormatsSupported = cpu_to_le16(FORMATS_SUPPORTED),
+ .dwNtbInMaxSize = cpu_to_le32(NTB_IN_SIZE),
+ .wNdpInDivisor = cpu_to_le16(NDP_IN_DIVISOR),
+ .wNdpInPayloadRemainder = cpu_to_le16(0),
+ .wNdpInAlignment = cpu_to_le16(4),
+
+ .dwNtbOutMaxSize = cpu_to_le32(NTB_OUT_SIZE),
+ .wNdpOutDivisor = cpu_to_le16(4),
+ .wNdpOutPayloadRemainder = cpu_to_le16(0),
+ .wNdpOutAlignment = cpu_to_le16(4),
+#ifdef CONFIG_ASR_TOE
+ .wNtbOutMaxDatagrams = 1,
+#else
+ .wNtbOutMaxDatagrams = 0, /* No limit */
+#endif
+};
+
+/*
+ * Here are options for the NCM Datagram Pointer table (NDP) parser.
+ * There are 2 different formats: NDP16 and NDP32 in the spec (ch. 3),
+ * in NDP16 offsets and sizes fields are 1 16bit word wide,
+ * in NDP32 -- 2 16bit words wide. Also signatures are different.
+ * To make the parser code the same, put the differences in the structure,
+ * and switch pointers to the structures when the format is changed.
+ */
+struct ndp_parser_opts {
+ u32 nth_sign;
+ u32 ndp_sign;
+ unsigned nth_size;
+ unsigned ndp_size;
+ unsigned ndplen_align;
+ /* sizes in u16 units */
+ unsigned dgram_item_len; /* index or length */
+ unsigned block_length;
+ unsigned fp_index;
+ unsigned reserved1;
+ unsigned reserved2;
+ unsigned next_fp_index;
+};
+
+#define INIT_NDP16_OPTS { \
+ .nth_sign = USB_CDC_NCM_NTH16_SIGN, \
+ .ndp_sign = USB_CDC_NCM_NDP16_NOCRC_SIGN, \
+ .nth_size = sizeof(struct usb_cdc_ncm_nth16), \
+ .ndp_size = sizeof(struct usb_cdc_ncm_ndp16), \
+ .ndplen_align = 4, \
+ .dgram_item_len = 1, \
+ .block_length = 1, \
+ .fp_index = 1, \
+ .reserved1 = 0, \
+ .reserved2 = 0, \
+ .next_fp_index = 1, \
+ }
+
+#define INIT_NDP32_OPTS { \
+ .nth_sign = USB_CDC_NCM_NTH32_SIGN, \
+ .ndp_sign = USB_CDC_NCM_NDP32_NOCRC_SIGN, \
+ .nth_size = sizeof(struct usb_cdc_ncm_nth32), \
+ .ndp_size = sizeof(struct usb_cdc_ncm_ndp32), \
+ .ndplen_align = 8, \
+ .dgram_item_len = 2, \
+ .block_length = 2, \
+ .fp_index = 2, \
+ .reserved1 = 1, \
+ .reserved2 = 2, \
+ .next_fp_index = 2, \
+ }
+
+static struct ndp_parser_opts ndp16_opts = INIT_NDP16_OPTS;
+static struct ndp_parser_opts ndp32_opts = INIT_NDP32_OPTS;
+
+/*---------------------------------------------------------------------------------*/
+/* USB descriptors */
+
+/* USB_DT_INTERFACE_ASSOCIATION: groups interfaces */
+static struct usb_interface_assoc_descriptor mbim_iad_desc = {
+ .bLength = sizeof mbim_iad_desc,
+ .bDescriptorType = USB_DT_INTERFACE_ASSOCIATION,
+
+ /* .bFirstInterface = DYNAMIC, */
+ .bInterfaceCount = 2, /* control + data */
+ .bFunctionClass = USB_CLASS_COMM,
+ .bFunctionSubClass = USB_CDC_SUBCLASS_MBIM,
+ .bFunctionProtocol = USB_CDC_PROTO_NONE,
+ /* .iFunction = DYNAMIC */
+};
+
+static struct usb_interface_assoc_descriptor ncm_iad_desc = {
+ .bLength = sizeof ncm_iad_desc,
+ .bDescriptorType = USB_DT_INTERFACE_ASSOCIATION,
+
+ /* .bFirstInterface = DYNAMIC, */
+ .bInterfaceCount = 2, /* control + data */
+ .bFunctionClass = USB_CLASS_COMM,
+ .bFunctionSubClass = USB_CDC_SUBCLASS_NCM,
+ .bFunctionProtocol = USB_CDC_PROTO_NONE,
+ /* .iFunction = DYNAMIC */
+};
+
+/* USB_DT_INTERFACE: Interface descriptor: CONTROL */
+static struct usb_interface_descriptor mbim_control_intf = {
+ .bLength = sizeof mbim_control_intf,
+ .bDescriptorType = USB_DT_INTERFACE,
+
+ /* .bInterfaceNumber = DYNAMIC */
+ .bNumEndpoints = 1,
+ .bInterfaceClass = USB_CLASS_COMM,
+ .bInterfaceSubClass = USB_CDC_SUBCLASS_MBIM,
+ .bInterfaceProtocol = USB_CDC_PROTO_NONE,
+ /* .iInterface = DYNAMIC */
+};
+
+static struct usb_interface_descriptor ncm_control_intf = {
+ .bLength = sizeof ncm_control_intf,
+ .bDescriptorType = USB_DT_INTERFACE,
+
+ /* .bInterfaceNumber = DYNAMIC */
+ .bNumEndpoints = 1,
+ .bInterfaceClass = USB_CLASS_COMM,
+ .bInterfaceSubClass = USB_CDC_SUBCLASS_NCM,
+ .bInterfaceProtocol = USB_CDC_PROTO_NONE,
+ /* .iInterface = DYNAMIC */
+};
+
+static struct usb_interface_descriptor ncm_mbim_control_intf = {
+ .bLength = sizeof mbim_control_intf,
+ .bDescriptorType = USB_DT_INTERFACE,
+
+ /* .bInterfaceNumber = DYNAMIC */
+ .bAlternateSetting = 1,
+ .bNumEndpoints = 1,
+ .bInterfaceClass = USB_CLASS_COMM,
+ .bInterfaceSubClass = USB_CDC_SUBCLASS_MBIM,
+ .bInterfaceProtocol = USB_CDC_PROTO_NONE,
+ /* .iInterface = DYNAMIC */
+};
+
+
+
+/* "Header Functional Descriptor" from CDC spec 5.2.3.1 */
+static struct usb_cdc_header_desc mbim_header_desc = {
+ .bLength = sizeof mbim_header_desc,
+ .bDescriptorType = USB_DT_CS_INTERFACE,
+ .bDescriptorSubType = USB_CDC_HEADER_TYPE,
+
+ .bcdCDC = cpu_to_le16(0x0110),
+};
+
+/* "Union Functional Descriptor" from CDC spec 5.2.3.8 */
+static struct usb_cdc_union_desc mbim_union_desc = {
+ .bLength = sizeof(mbim_union_desc),
+ .bDescriptorType = USB_DT_CS_INTERFACE,
+ .bDescriptorSubType = USB_CDC_UNION_TYPE,
+ /* .bMasterInterface0 = DYNAMIC */
+ /* .bSlaveInterface0 = DYNAMIC */
+};
+
+/* "Ethernet Networking Functional Descriptor" from CDC spec 5.2.3.16 */
+static struct usb_cdc_ether_desc ether_desc = {
+ .bLength = sizeof ether_desc,
+ .bDescriptorType = USB_DT_CS_INTERFACE,
+ .bDescriptorSubType = USB_CDC_ETHERNET_TYPE,
+
+ /* this descriptor actually adds value, surprise! */
+ /* .iMACAddress = DYNAMIC */
+ .bmEthernetStatistics = cpu_to_le32(0), /* no statistics */
+ .wMaxSegmentSize = cpu_to_le16(ETH_FRAME_LEN),
+ .wNumberMCFilters = cpu_to_le16(0),
+ .bNumberPowerFilters = 0,
+};
+
+/* "MBIM Control Model Functional Descriptor" */
+static struct usb_cdc_mbim_desc mbim_desc = {
+ .bLength = sizeof mbim_desc,
+ .bDescriptorType = USB_DT_CS_INTERFACE,
+ .bDescriptorSubType = USB_CDC_MBIM_TYPE,
+
+ .bcdMBIMVersion = cpu_to_le16(0x0100),
+ .wMaxControlMessage = cpu_to_le16(MAX_CTRL_PKT_SIZE),
+ .bNumberFilters = 0x10,
+ .bMaxFilterSize = 0x80,
+ .wMaxSegmentSize = cpu_to_le16(0xfe0),
+ .bmNetworkCapabilities = 0x20,
+};
+
+/* "MBIM Control Model Extended Functional Descriptor" */
+static struct usb_cdc_mbim_ext_desc mbim_ext_desc = {
+ .bLength = sizeof mbim_ext_desc,
+ .bDescriptorType = USB_DT_CS_INTERFACE,
+ .bDescriptorSubType = USB_CDC_MBIM_EXT_TYPE,
+
+ .bcdMBIMExtendedVersion = cpu_to_le16(0x0100),
+ .bMaxOutstandingCommandMessages = 1,
+ .wMTU = cpu_to_le16(1500),
+};
+
+#define NCAPS (USB_CDC_NCM_NCAP_ETH_FILTER)
+
+static struct usb_cdc_ncm_desc ncm_desc = {
+ .bLength = sizeof ncm_desc,
+ .bDescriptorType = USB_DT_CS_INTERFACE,
+ .bDescriptorSubType = USB_CDC_NCM_TYPE,
+
+ .bcdNcmVersion = cpu_to_le16(0x0100),
+ /* can process SetEthernetPacketFilter */
+ .bmNetworkCapabilities = NCAPS,
+};
+
+
+/* USB_DT_INTERFACE: Interface descriptor: DATA disable */
+static struct usb_interface_descriptor mbim_data_nop_intf = {
+ .bLength = sizeof mbim_data_nop_intf,
+ .bDescriptorType = USB_DT_INTERFACE,
+
+ /* .bInterfaceNumber = DYNAMIC */
+ .bAlternateSetting = 0,
+ .bNumEndpoints = 0,
+ .bInterfaceClass = USB_CLASS_CDC_DATA,
+ .bInterfaceSubClass = 0,
+ .bInterfaceProtocol = USB_CDC_MBIM_PROTO_NTB,
+ /* .iInterface = DYNAMIC */
+};
+
+static struct usb_interface_descriptor ncm_data_nop_intf = {
+ .bLength = sizeof ncm_data_nop_intf,
+ .bDescriptorType = USB_DT_INTERFACE,
+
+ .bInterfaceNumber = 1,
+ .bAlternateSetting = 0,
+ .bNumEndpoints = 0,
+ .bInterfaceClass = USB_CLASS_CDC_DATA,
+ .bInterfaceSubClass = 0,
+ .bInterfaceProtocol = USB_CDC_NCM_PROTO_NTB,
+ /* .iInterface = DYNAMIC */
+};
+
+/* USB_DT_INTERFACE: Interface descriptor: DATA enable */
+static struct usb_interface_descriptor mbim_data_intf = {
+ .bLength = sizeof mbim_data_intf,
+ .bDescriptorType = USB_DT_INTERFACE,
+
+ /* .bInterfaceNumber = DYNAMIC */
+ .bAlternateSetting = 1,
+ .bNumEndpoints = 2,
+ .bInterfaceClass = USB_CLASS_CDC_DATA,
+ .bInterfaceSubClass = 0,
+ .bInterfaceProtocol = USB_CDC_MBIM_PROTO_NTB,
+ /* .iInterface = DYNAMIC */
+};
+
+static struct usb_interface_descriptor ncm_data_intf = {
+ .bLength = sizeof ncm_data_intf,
+ .bDescriptorType = USB_DT_INTERFACE,
+
+ .bInterfaceNumber = 1,
+ .bAlternateSetting = 1,
+ .bNumEndpoints = 2,
+ .bInterfaceClass = USB_CLASS_CDC_DATA,
+ .bInterfaceSubClass = 0,
+ .bInterfaceProtocol = USB_CDC_NCM_PROTO_NTB,
+ /* .iInterface = DYNAMIC */
+};
+
+static struct usb_interface_descriptor ncm_mbim_data_intf = {
+ .bLength = sizeof mbim_data_intf,
+ .bDescriptorType = USB_DT_INTERFACE,
+
+ /* .bInterfaceNumber = DYNAMIC */
+ .bAlternateSetting = 2,
+ .bNumEndpoints = 2,
+ .bInterfaceClass = USB_CLASS_CDC_DATA,
+ .bInterfaceSubClass = 0,
+ .bInterfaceProtocol = USB_CDC_MBIM_PROTO_NTB,
+ /* .iInterface = DYNAMIC */
+};
+
+/* full speed endpoints support: */
+
+/* USB_DT_ENDPOINT: Endpoint descriptor: NOTIFY */
+static struct usb_endpoint_descriptor fs_mbim_notify_desc = {
+ .bLength = USB_DT_ENDPOINT_SIZE,
+ .bDescriptorType = USB_DT_ENDPOINT,
+
+ .bEndpointAddress = USB_DIR_IN,
+ .bmAttributes = USB_ENDPOINT_XFER_INT,
+ .wMaxPacketSize = cpu_to_le16(MBIM_STATUS_BYTECOUNT),
+ .bInterval = 1 << LOG2_STATUS_INTERVAL_MSEC,
+};
+
+/* USB_DT_ENDPOINT: Endpoint descriptor: IN */
+static struct usb_endpoint_descriptor fs_mbim_in_desc = {
+ .bLength = USB_DT_ENDPOINT_SIZE,
+ .bDescriptorType = USB_DT_ENDPOINT,
+
+ .bEndpointAddress = USB_DIR_IN,
+ .bmAttributes = USB_ENDPOINT_XFER_BULK,
+};
+
+/* USB_DT_ENDPOINT: Endpoint descriptor: OUT */
+static struct usb_endpoint_descriptor fs_mbim_out_desc = {
+ .bLength = USB_DT_ENDPOINT_SIZE,
+ .bDescriptorType = USB_DT_ENDPOINT,
+
+ .bEndpointAddress = USB_DIR_OUT,
+ .bmAttributes = USB_ENDPOINT_XFER_BULK,
+};
+
+/* List of all the full speed supported descriptors */
+static struct usb_descriptor_header *mbim_fs_function[] = {
+ (struct usb_descriptor_header *) &mbim_iad_desc,
+ /* CDC MBIM control descriptors */
+ (struct usb_descriptor_header *) &mbim_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &mbim_desc,
+ (struct usb_descriptor_header *) &mbim_ext_desc,
+ (struct usb_descriptor_header *) &fs_mbim_notify_desc,
+ /* data interface, altsettings 0 and 1 */
+ (struct usb_descriptor_header *) &mbim_data_nop_intf,
+ (struct usb_descriptor_header *) &mbim_data_intf,
+ (struct usb_descriptor_header *) &fs_mbim_in_desc,
+ (struct usb_descriptor_header *) &fs_mbim_out_desc,
+ NULL,
+};
+
+static struct usb_descriptor_header *ncm_fs_function[] = {
+ (struct usb_descriptor_header *) &ncm_iad_desc,
+ /* CDC NCM control descriptors */
+ (struct usb_descriptor_header *) &ncm_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &ncm_desc,
+ (struct usb_descriptor_header *) &fs_mbim_notify_desc,
+ /* data interface, altsettings 0 and 1 */
+ (struct usb_descriptor_header *) &ncm_data_nop_intf,
+ (struct usb_descriptor_header *) &ncm_data_intf,
+ (struct usb_descriptor_header *) &fs_mbim_in_desc,
+ (struct usb_descriptor_header *) &fs_mbim_out_desc,
+ NULL,
+};
+
+static struct usb_descriptor_header *ncm_mbim_fs_function[] = {
+ (struct usb_descriptor_header *) &ncm_iad_desc,
+ /* CDC NCM control descriptors alt 0 */
+ (struct usb_descriptor_header *) &ncm_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &ncm_desc,
+ (struct usb_descriptor_header *) &fs_mbim_notify_desc,
+ /* CDC MBIM control descriptors alt 1 */
+ (struct usb_descriptor_header *) &ncm_mbim_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &mbim_desc,
+ (struct usb_descriptor_header *) &mbim_ext_desc,
+ (struct usb_descriptor_header *) &fs_mbim_notify_desc,
+
+ /* data interface, altsettings 0, 1 and 2 */
+ (struct usb_descriptor_header *) &ncm_data_nop_intf,
+ (struct usb_descriptor_header *) &ncm_data_intf,
+ (struct usb_descriptor_header *) &fs_mbim_in_desc,
+ (struct usb_descriptor_header *) &fs_mbim_out_desc,
+ (struct usb_descriptor_header *) &ncm_mbim_data_intf,
+ (struct usb_descriptor_header *) &fs_mbim_in_desc,
+ (struct usb_descriptor_header *) &fs_mbim_out_desc,
+ NULL,
+};
+
+/* high speed endpoints support: */
+
+/* USB_DT_ENDPOINT: Endpoint descriptor: NOTIFY */
+static struct usb_endpoint_descriptor hs_mbim_notify_desc = {
+ .bLength = USB_DT_ENDPOINT_SIZE,
+ .bDescriptorType = USB_DT_ENDPOINT,
+
+ .bEndpointAddress = USB_DIR_IN,
+ .bmAttributes = USB_ENDPOINT_XFER_INT,
+ .wMaxPacketSize = cpu_to_le16(MBIM_STATUS_BYTECOUNT),
+ .bInterval = LOG2_STATUS_INTERVAL_MSEC + 4,
+};
+
+/* USB_DT_ENDPOINT: Endpoint descriptor: IN */
+static struct usb_endpoint_descriptor hs_mbim_in_desc = {
+ .bLength = USB_DT_ENDPOINT_SIZE,
+ .bDescriptorType = USB_DT_ENDPOINT,
+
+ .bEndpointAddress = USB_DIR_IN,
+ .bmAttributes = USB_ENDPOINT_XFER_BULK,
+ .wMaxPacketSize = cpu_to_le16(512),
+};
+
+/* USB_DT_ENDPOINT: Endpoint descriptor: OUT */
+static struct usb_endpoint_descriptor hs_mbim_out_desc = {
+ .bLength = USB_DT_ENDPOINT_SIZE,
+ .bDescriptorType = USB_DT_ENDPOINT,
+
+ .bEndpointAddress = USB_DIR_OUT,
+ .bmAttributes = USB_ENDPOINT_XFER_BULK,
+ .wMaxPacketSize = cpu_to_le16(512),
+};
+
+/* List of all the high speed supported descriptors */
+static struct usb_descriptor_header *mbim_hs_function[] = {
+ (struct usb_descriptor_header *) &mbim_iad_desc,
+ /* CDC MBIM control descriptors */
+ (struct usb_descriptor_header *) &mbim_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &mbim_desc,
+ (struct usb_descriptor_header *) &mbim_ext_desc,
+ (struct usb_descriptor_header *) &hs_mbim_notify_desc,
+ /* data interface, altsettings 0 and 1 */
+ (struct usb_descriptor_header *) &mbim_data_nop_intf,
+ (struct usb_descriptor_header *) &mbim_data_intf,
+ (struct usb_descriptor_header *) &hs_mbim_in_desc,
+ (struct usb_descriptor_header *) &hs_mbim_out_desc,
+ NULL,
+};
+
+static struct usb_descriptor_header *ncm_hs_function[] = {
+ (struct usb_descriptor_header *) &ncm_iad_desc,
+ /* CDC NCM control descriptors */
+ (struct usb_descriptor_header *) &ncm_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &ncm_desc,
+ (struct usb_descriptor_header *) &hs_mbim_notify_desc,
+ /* data interface, altsettings 0 and 1 */
+ (struct usb_descriptor_header *) &ncm_data_nop_intf,
+ (struct usb_descriptor_header *) &ncm_data_intf,
+ (struct usb_descriptor_header *) &hs_mbim_in_desc,
+ (struct usb_descriptor_header *) &hs_mbim_out_desc,
+ NULL,
+};
+
+static struct usb_descriptor_header *ncm_mbim_hs_function[] = {
+ (struct usb_descriptor_header *) &ncm_iad_desc,
+ /* CDC NCM control descriptors */
+ (struct usb_descriptor_header *) &ncm_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &ncm_desc,
+ (struct usb_descriptor_header *) &hs_mbim_notify_desc,
+
+ (struct usb_descriptor_header *) &ncm_mbim_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &mbim_desc,
+ (struct usb_descriptor_header *) &mbim_ext_desc,
+ (struct usb_descriptor_header *) &hs_mbim_notify_desc,
+ /* data interface, altsettings 0 and 1 */
+ (struct usb_descriptor_header *) &ncm_data_nop_intf,
+ (struct usb_descriptor_header *) &ncm_data_intf,
+ (struct usb_descriptor_header *) &hs_mbim_in_desc,
+ (struct usb_descriptor_header *) &hs_mbim_out_desc,
+ (struct usb_descriptor_header *) &ncm_mbim_data_intf,
+ (struct usb_descriptor_header *) &hs_mbim_in_desc,
+ (struct usb_descriptor_header *) &hs_mbim_out_desc,
+ NULL,
+};
+
+/* super speed endpoints support: */
+
+/* USB_DT_ENDPOINT: Endpoint descriptor: NOTIFY */
+static struct usb_endpoint_descriptor ss_mbim_notify_desc = {
+ .bLength = USB_DT_ENDPOINT_SIZE,
+ .bDescriptorType = USB_DT_ENDPOINT,
+
+ .bEndpointAddress = USB_DIR_IN,
+ .bmAttributes = USB_ENDPOINT_XFER_INT,
+ .wMaxPacketSize = cpu_to_le16(MBIM_STATUS_BYTECOUNT),
+ .bInterval = LOG2_STATUS_INTERVAL_MSEC + 4,
+};
+
+static struct usb_ss_ep_comp_descriptor ss_mbim_intr_comp_desc = {
+ .bLength = sizeof ss_mbim_intr_comp_desc,
+ .bDescriptorType = USB_DT_SS_ENDPOINT_COMP,
+
+ /* the following 3 values can be tweaked if necessary */
+ /* .bMaxBurst = 0, */
+ /* .bmAttributes = 0, */
+ .wBytesPerInterval = cpu_to_le16(MBIM_STATUS_BYTECOUNT),
+};
+
+/* USB_DT_ENDPOINT: Endpoint descriptor: IN */
+static struct usb_endpoint_descriptor ss_mbim_in_desc = {
+ .bLength = USB_DT_ENDPOINT_SIZE,
+ .bDescriptorType = USB_DT_ENDPOINT,
+
+ .bEndpointAddress = USB_DIR_IN,
+ .bmAttributes = USB_ENDPOINT_XFER_BULK,
+ .wMaxPacketSize = cpu_to_le16(1024),
+};
+
+/* USB_DT_ENDPOINT: Endpoint descriptor: OUT */
+static struct usb_endpoint_descriptor ss_mbim_out_desc = {
+ .bLength = USB_DT_ENDPOINT_SIZE,
+ .bDescriptorType = USB_DT_ENDPOINT,
+
+ .bEndpointAddress = USB_DIR_OUT,
+ .bmAttributes = USB_ENDPOINT_XFER_BULK,
+ .wMaxPacketSize = cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor ss_mbim_bulk_comp_desc = {
+ .bLength = sizeof ss_mbim_bulk_comp_desc,
+ .bDescriptorType = USB_DT_SS_ENDPOINT_COMP,
+
+ /* the following 2 values can be tweaked if necessary */
+#ifdef CONFIG_ASR_TOE
+ .bMaxBurst = 15,
+#endif
+ /* .bmAttributes = 0, */
+};
+
+/* List of all the supper speed supported descriptors */
+static struct usb_descriptor_header *mbim_ss_function[] = {
+ (struct usb_descriptor_header *) &mbim_iad_desc,
+ /* CDC MBIM control descriptors */
+ (struct usb_descriptor_header *) &mbim_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &mbim_desc,
+ (struct usb_descriptor_header *) &mbim_ext_desc,
+ (struct usb_descriptor_header *) &ss_mbim_notify_desc,
+ (struct usb_descriptor_header *) &ss_mbim_intr_comp_desc,
+ /* data interface, altsettings 0 and 1 */
+ (struct usb_descriptor_header *) &mbim_data_nop_intf,
+ (struct usb_descriptor_header *) &mbim_data_intf,
+ (struct usb_descriptor_header *) &ss_mbim_in_desc,
+ (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
+ (struct usb_descriptor_header *) &ss_mbim_out_desc,
+ (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
+ NULL,
+};
+
+static struct usb_descriptor_header *ncm_ss_function[] = {
+ (struct usb_descriptor_header *) &ncm_iad_desc,
+ /* CDC NCM control descriptors */
+ (struct usb_descriptor_header *) &ncm_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &ncm_desc,
+ (struct usb_descriptor_header *) &ss_mbim_notify_desc,
+ (struct usb_descriptor_header *) &ss_mbim_intr_comp_desc,
+ /* data interface, altsettings 0 and 1 */
+ (struct usb_descriptor_header *) &ncm_data_nop_intf,
+ (struct usb_descriptor_header *) &ncm_data_intf,
+ (struct usb_descriptor_header *) &ss_mbim_in_desc,
+ (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
+ (struct usb_descriptor_header *) &ss_mbim_out_desc,
+ (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
+ NULL,
+};
+
+static struct usb_descriptor_header *ncm_mbim_ss_function[] = {
+ (struct usb_descriptor_header *) &ncm_iad_desc,
+ /* CDC NCM control descriptors */
+ (struct usb_descriptor_header *) &ncm_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &ncm_desc,
+ (struct usb_descriptor_header *) &ss_mbim_notify_desc,
+ (struct usb_descriptor_header *) &ss_mbim_intr_comp_desc,
+ (struct usb_descriptor_header *) &ncm_mbim_control_intf,
+ (struct usb_descriptor_header *) &mbim_header_desc,
+ (struct usb_descriptor_header *) &mbim_union_desc,
+ (struct usb_descriptor_header *) ðer_desc,
+ (struct usb_descriptor_header *) &mbim_desc,
+ (struct usb_descriptor_header *) &mbim_ext_desc,
+ (struct usb_descriptor_header *) &ss_mbim_notify_desc,
+ (struct usb_descriptor_header *) &ss_mbim_intr_comp_desc,
+ /* data interface, altsettings 0 and 1 */
+ (struct usb_descriptor_header *) &ncm_data_nop_intf,
+ (struct usb_descriptor_header *) &ncm_data_intf,
+ (struct usb_descriptor_header *) &ss_mbim_in_desc,
+ (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
+ (struct usb_descriptor_header *) &ss_mbim_out_desc,
+ (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
+ (struct usb_descriptor_header *) &ncm_mbim_data_intf,
+ (struct usb_descriptor_header *) &ss_mbim_in_desc,
+ (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
+ (struct usb_descriptor_header *) &ss_mbim_out_desc,
+ (struct usb_descriptor_header *) &ss_mbim_bulk_comp_desc,
+ NULL,
+};
+
+
+/*---------------------------------------------------------------------------------*/
+/* string descriptors: */
+
+#define STRING_CTRL_IDX 0
+#define STRING_MAC_IDX 1
+#define STRING_DATA_IDX 2
+#define STRING_IAD_IDX 3
+
+static struct usb_string mbim_string_defs[] = {
+ [STRING_CTRL_IDX].s = "MBIM Control",
+ [STRING_MAC_IDX].s = NULL /* DYNAMIC */,
+ [STRING_DATA_IDX].s = "MBIM Data",
+ [STRING_IAD_IDX].s = "CDC MBIM",
+ { } /* end of list */
+};
+
+static struct usb_gadget_strings mbim_string_table = {
+ .language = 0x0409, /* en-us */
+ .strings = mbim_string_defs,
+};
+
+static struct usb_gadget_strings mbim_string_table_2 = {
+ .language = 0,
+ .strings = mbim_string_defs,
+};
+
+static struct usb_gadget_strings *mbim_strings[] = {
+ &mbim_string_table,
+ &mbim_string_table_2,
+ NULL,
+};
+
+/*Microsoft defined the following OS Descriptor*/
+#define USB_VENDOR_OSVC_DEFAULT 0x04
+#define OS_STRING_ID 0xEE /*Microsoft defined*/
+#define MS_EXTENDED_COMPAT_ID 0x04 /*Microsoft defined*/
+#define MS_EXTENDED_PROPERTY 0x05 /*Microsoft defined*/
+
+/* Microsoft MBIM OS String */
+static u8 mbim_os_string[] = {
+ 18, /* sizeof(mtp_os_string) */
+ USB_DT_STRING,
+ /* Signature field: "MSFT100" */
+ 'M', 0, 'S', 0, 'F', 0, 'T', 0, '1', 0, '0', 0, '0', 0,
+ /* vendor code */
+ USB_VENDOR_OSVC_DEFAULT,
+ /* padding */
+ 0
+};
+
+/* Microsoft Extended Configuration Descriptor Header Section */
+struct mbim_ext_config_desc_header {
+ __le32 dwLength;
+ __u16 bcdVersion;
+ __le16 wIndex;
+ __u8 bCount;
+ __u8 reserved[7];
+};
+
+/* Microsoft Extended Configuration Descriptor Function Section */
+struct mbim_ext_config_desc_function {
+ __u8 bFirstInterfaceNumber;
+ __u8 bInterfaceCount;
+ __u8 compatibleID[8];
+ __u8 subCompatibleID[8];
+ __u8 reserved[6];
+};
+
+/* MBIM Extended Configuration Descriptor */
+struct {
+ struct mbim_ext_config_desc_header header;
+ struct mbim_ext_config_desc_function function;
+} mbim_ext_config_desc = {
+ .header = {
+ .dwLength = __constant_cpu_to_le32(sizeof(mbim_ext_config_desc)),
+ .bcdVersion = __constant_cpu_to_le16(0x0100),
+ .wIndex = __constant_cpu_to_le16(4),
+ .bCount = __constant_cpu_to_le16(1),
+ },
+ .function = {
+ .bFirstInterfaceNumber = 0,
+ .bInterfaceCount = 2,
+ .compatibleID = { 'M', 'B', 'I', 'M' },
+ },
+};
+
+#define MBIM_MAX_RESP_Q (64)
+/*-----------------------------------------------------------------------*/
+
+static inline void put_mbim(__le16 **p, unsigned size, unsigned val)
+{
+ switch (size) {
+ case 1:
+ put_unaligned_le16((u16)val, *p);
+ break;
+ case 2:
+ put_unaligned_le32((u32)val, *p);
+ break;
+ default:
+ BUG();
+ }
+
+ *p += size;
+}
+
+static inline void put_mbim_no_inc(void *p, unsigned size, unsigned val)
+{
+ switch (size) {
+ case 1:
+ put_unaligned_le16((u16)val, p);
+ break;
+ case 2:
+ put_unaligned_le32((u32)val, p);
+ break;
+ default:
+ BUG();
+ }
+}
+
+static inline unsigned get_mbim(__le16 **p, unsigned size)
+{
+ unsigned tmp;
+
+ switch (size) {
+ case 1:
+ tmp = get_unaligned_le16(*p);
+ break;
+ case 2:
+ tmp = get_unaligned_le32(*p);
+ break;
+ default:
+ BUG();
+ }
+
+ *p += size;
+ return tmp;
+}
+
+/*-------------------------------------------------------------------------*/
+static inline int mbim_lock(atomic_t *excl)
+{
+ if (atomic_inc_return(excl) == 1) {
+ return 0;
+ } else {
+ atomic_dec(excl);
+ return -EBUSY;
+ }
+}
+
+static inline void mbim_unlock(atomic_t *excl)
+{
+ atomic_dec(excl);
+}
+
+static struct ctrl_pkt *mbim_alloc_ctrl_pkt(unsigned len, gfp_t flags)
+{
+ struct ctrl_pkt *pkt;
+
+ pkt = kzalloc(sizeof(struct ctrl_pkt), flags);
+ if (!pkt)
+ return NULL;
+
+ pkt->buf = kmalloc(len, flags);
+ if (!pkt->buf) {
+ kfree(pkt);
+ return NULL;
+ }
+ pkt->len = len;
+
+ return pkt;
+}
+
+static void mbim_free_ctrl_pkt(struct ctrl_pkt *pkt)
+{
+ if (pkt) {
+ kfree(pkt->buf);
+ kfree(pkt);
+ }
+}
+
+static struct usb_request *mbim_alloc_req(struct usb_ep *ep, int buffer_size)
+{
+ struct usb_request *req = usb_ep_alloc_request(ep, GFP_KERNEL);
+ if (!req)
+ return NULL;
+
+ req->buf = kmalloc(buffer_size, GFP_KERNEL);
+ if (!req->buf) {
+ usb_ep_free_request(ep, req);
+ return NULL;
+ }
+ req->length = buffer_size;
+ return req;
+}
+
+void fmbim_free_req(struct usb_ep *ep, struct usb_request *req)
+{
+ if (req) {
+ kfree(req->buf);
+ usb_ep_free_request(ep, req);
+ }
+}
+
+static void fmbim_ctrl_response_available_with_lock(struct f_mbim *dev, char need_lock)
+{
+ struct usb_request *req = dev->notify_req;
+ struct usb_cdc_notification *event = NULL;
+ unsigned long flags;
+ int ret;
+ struct ctrl_pkt *cpkt = NULL;
+
+ int notif_c = 0;
+
+ if (need_lock)
+ spin_lock_irqsave(&dev->lock, flags);
+
+ if (!atomic_read(&dev->online)) {
+ pr_info("dev:%p is not online\n", dev);
+ if (need_lock)
+ spin_unlock_irqrestore(&dev->lock, flags);
+ return;
+ }
+
+ if (!req) {
+ /* Hold up to 64 responses in the circular queue */
+ atomic_inc(&dev->response_available);
+ pr_info_ratelimited("notify_req is NULL, try to queue the response, increment response available to %d\n",
+ atomic_read(&dev->response_available));
+ if (atomic_read(&dev->response_available) > MBIM_MAX_RESP_Q) {
+ cpkt = list_first_entry(&dev->cpkt_resp_q, struct ctrl_pkt, list);
+ BUG_ON(!cpkt);
+ list_del(&cpkt->list);
+ pr_info("cpkt=%p, transaction id=%d, removed from response queue head (too much responses)"
+ , cpkt, cpkt->transaction_id);
+ mbim_free_ctrl_pkt(cpkt);
+ atomic_dec(&dev->response_available);
+ }
+ if (need_lock)
+ spin_unlock_irqrestore(&dev->lock, flags);
+ return;
+ }
+
+ if (!req->buf) {
+ pr_info("dev:%p req->buf is NULL\n", dev);
+ if (need_lock)
+ spin_unlock_irqrestore(&dev->lock, flags);
+ return;
+ }
+
+ notif_c = atomic_inc_return(&dev->notify_count);
+ pr_info_ratelimited("atomic_inc_return[notif_c] = %d\n", notif_c);
+
+ event = req->buf;
+ event->bmRequestType = USB_DIR_IN | USB_TYPE_CLASS
+ | USB_RECIP_INTERFACE;
+ event->bNotificationType = USB_CDC_NOTIFY_RESPONSE_AVAILABLE;
+ event->wValue = cpu_to_le16(0);
+ event->wIndex = cpu_to_le16(dev->ctrl_id);
+ event->wLength = cpu_to_le16(0);
+ dev->notify_req = NULL;
+
+ if (need_lock)
+ spin_unlock_irqrestore(&dev->lock, flags);
+
+ pr_debug("Call usb_ep_queue\n");
+
+ ret = usb_ep_queue(dev->notify, req, GFP_ATOMIC);
+ if (ret) {
+ dev->notify_req = req;
+ atomic_dec(&dev->notify_count);
+ pr_err("ep enqueue error %d\n", ret);
+ }
+}
+static void fmbim_ctrl_response_available(struct f_mbim *dev)
+{
+ /* Tell fmbim_ctrl_response_available_ext to use lock */
+ fmbim_ctrl_response_available_with_lock(dev,1);
+}
+
+static int
+fmbim_send_cpkt_response(struct f_mbim *gr, struct ctrl_pkt *cpkt)
+{
+ struct f_mbim *dev = gr;
+ unsigned long flags;
+
+ if (!gr || !cpkt) {
+ pr_err("Invalid cpkt, dev:%p cpkt:%p\n",
+ gr, cpkt);
+ return -ENODEV;
+ }
+
+ if (!atomic_read(&dev->online)) {
+ pr_info("dev:%p is not connected\n", dev);
+ mbim_free_ctrl_pkt(cpkt);
+ return 0;
+ }
+
+ pr_debug("Add packet to cpkt_resp_q trans_id=%d\n", cpkt->transaction_id);
+ spin_lock_irqsave(&dev->lock, flags);
+ list_add_tail(&cpkt->list, &dev->cpkt_resp_q);
+ spin_unlock_irqrestore(&dev->lock, flags);
+
+ fmbim_ctrl_response_available(dev);
+
+ return 0;
+}
+
+static void mbim_reset_values(struct f_mbim *mbim)
+{
+ mbim->parser_opts = &ndp16_opts;
+ mbim->is_crc = false;
+ mbim->port.cdc_filter = DEFAULT_FILTER;
+ mbim->ntb_input_size = le32_to_cpu(ntb_parameters.dwNtbInMaxSize);;
+ mbim->port.header_len = USB_CDC_NCM_MIN_HEADER16_SIZE;
+ atomic_set(&mbim->notify_count, 0);
+ atomic_set(&mbim->response_available, 0);
+ atomic_set(&mbim->online, 0);
+ mbim->port.fixed_out_len = le32_to_cpu(ntb_parameters.dwNtbOutMaxSize);
+ mbim->port.fixed_in_len = le32_to_cpu(ntb_parameters.dwNtbInMaxSize);
+}
+
+static void mbim_reset_function_queue(struct f_mbim *dev)
+{
+ struct ctrl_pkt *cpkt = NULL;
+
+ spin_lock(&dev->lock);
+
+ cpkt = mbim_alloc_ctrl_pkt(0, GFP_ATOMIC);
+ if (!cpkt) {
+ pr_err("Unable to allocate reset function pkt\n");
+ spin_unlock(&dev->lock);
+ return;
+ }
+
+ list_add_tail(&cpkt->list, &dev->cpkt_req_q);
+ spin_unlock(&dev->lock);
+
+ wake_up(&dev->read_wq);
+}
+
+static void fmbim_reset_cmd_complete(struct usb_ep *ep, struct usb_request *req)
+{
+ struct f_mbim *dev = req->context;
+
+ mbim_reset_function_queue(dev);
+}
+
+static void mbim_clear_queues(struct f_mbim *mbim)
+{
+ struct ctrl_pkt *cpkt = NULL;
+ struct list_head *act, *tmp;
+
+ spin_lock(&mbim->lock);
+ list_for_each_safe(act, tmp, &mbim->cpkt_req_q) {
+ cpkt = list_entry(act, struct ctrl_pkt, list);
+ list_del(&cpkt->list);
+ mbim_free_ctrl_pkt(cpkt);
+ }
+ list_for_each_safe(act, tmp, &mbim->cpkt_resp_q) {
+ cpkt = list_entry(act, struct ctrl_pkt, list);
+ list_del(&cpkt->list);
+ mbim_free_ctrl_pkt(cpkt);
+ }
+ spin_unlock(&mbim->lock);
+}
+/*
+ * Context: mbim->lock held
+ */
+static void mbim_do_notify(struct f_mbim *mbim)
+{
+ struct usb_request *req = mbim->notify_req;
+ struct usb_cdc_notification *event;
+ struct usb_composite_dev *cdev = mbim->port.func.config->cdev;
+ struct usb_cdc_speed_change *speed_data;
+ int status;
+
+ pr_info("state = %d\n", mbim->notify_state);
+
+ if (unlikely(!req)) {
+ pr_err("No request available\n");
+ return;
+ }
+
+ event = req->buf;
+ switch (mbim->notify_state) {
+ case MBIM_NOTIFY_NONE:
+ return;
+
+ case MBIM_NOTIFY_CONNECT:
+ event->bNotificationType = USB_CDC_NOTIFY_NETWORK_CONNECTION;
+ if (mbim->is_open)
+ event->wValue = cpu_to_le16(1);
+ else
+ event->wValue = cpu_to_le16(0);
+ event->wLength = 0;
+ req->length = sizeof(struct usb_cdc_notification);
+
+ pr_info("notify connect %s\n", mbim->is_open ? "true" : "false");
+ mbim->notify_state = MBIM_NOTIFY_NONE;
+ break;
+
+ case MBIM_NOTIFY_SPEED:
+ event->bNotificationType = USB_CDC_NOTIFY_SPEED_CHANGE;
+ event->wValue = cpu_to_le16(0);
+ event->wLength = cpu_to_le16(8);
+
+ /* SPEED_CHANGE data is up/down speeds in bits/sec */
+ speed_data = (req->buf + sizeof(struct usb_cdc_notification));
+ speed_data->DLBitRRate = cpu_to_le32(mbim_bitrate(cdev->gadget));
+ speed_data->ULBitRate = speed_data->DLBitRRate;
+
+ req->length = (sizeof(struct usb_cdc_notification) + sizeof(struct usb_cdc_speed_change));
+
+ pr_info("notify speed %d bps\n", mbim_bitrate(cdev->gadget));
+ mbim->notify_state = MBIM_NOTIFY_CONNECT;
+ break;
+ }
+ event->bmRequestType = 0xA1;
+ event->wIndex = cpu_to_le16(mbim->ctrl_id);
+
+ atomic_inc(&mbim->notify_count);
+ pr_info("notif_c = %d\n", atomic_read(&mbim->notify_count));
+ mbim->notify_req = NULL;
+ /*
+ * In double buffering if there is a space in FIFO,
+ * completion callback can be called right after the call,
+ * so unlocking
+ */
+ spin_unlock(&mbim->lock);
+ status = usb_ep_queue(mbim->notify, req, GFP_ATOMIC);
+ spin_lock(&mbim->lock);
+ if (status < 0) {
+ mbim->notify_req = req;
+ atomic_dec(&mbim->notify_count);
+ mbim->notify_state = MBIM_NOTIFY_NONE;
+ pr_err("usb_ep_queue failed, err: %d\n", status);
+ }
+}
+
+/*
+ * Context: mbim->lock held
+ */
+static void mbim_notify(struct f_mbim *mbim)
+{
+ /*
+ * NOTE on most versions of Linux, host side cdc-ethernet
+ * won't listen for notifications until its netdevice opens.
+ * The first notification then sits in the FIFO for a long
+ * time, and the second one is queued.
+ *
+ * If mbim_notify() is called before the second (CONNECT)
+ * notification is sent, then it will reset to send the SPEED
+ * notificaion again (and again, and again), but it's not a problem
+ */
+ if (mbim->notify_state == MBIM_NOTIFY_NONE) {
+ mbim->notify_state = MBIM_NOTIFY_SPEED;
+ mbim_do_notify(mbim);
+ } else {
+ pr_info("skipped, notify in progress (state=%d)\n",
+ mbim->notify_state);
+ }
+}
+
+static void mbim_notify_complete(struct usb_ep *ep, struct usb_request *req)
+{
+ struct f_mbim *mbim = req->context;
+ struct usb_cdc_notification *event = req->buf;
+ int notif_c;
+
+ spin_lock(&mbim->lock);
+ switch (req->status) {
+ case 0:
+ pr_info_ratelimited("Notification 0x%02x sent\n", event->bNotificationType);
+ if (unlikely(!atomic_read(&mbim->online))) {
+ pr_info("%s: dev is offline\n", __func__);
+ pr_info("notif_c = %d\n", atomic_read(&mbim->notify_count));
+ mbim->notify_state = MBIM_NOTIFY_NONE;
+ /* atomic_set(&mbim->notify_count, 0); */
+ mbim->notify_req = req;
+ spin_unlock(&mbim->lock);
+ return;
+ }
+
+ notif_c = atomic_dec_return(&mbim->notify_count);
+ if (unlikely(notif_c)) {
+ pr_err("notification count != 0 (%d)\n", notif_c);
+ BUG();
+ }
+
+ if (event->bNotificationType == USB_CDC_NOTIFY_RESPONSE_AVAILABLE) {
+ BUG_ON(mbim->notify_state != MBIM_NOTIFY_NONE);
+ mbim->notify_req = req;
+#ifndef SERIAL_RESPONSE
+ if (atomic_read(&mbim->response_available)) {
+ pr_debug("Response available=%d (was busy"
+ "before), try to send it again.\n",
+ atomic_read(&mbim->response_available));
+ atomic_dec(&mbim->response_available);
+ fmbim_ctrl_response_available_with_lock(mbim,0);
+ }
+#endif
+ spin_unlock(&mbim->lock);
+ return;
+ } else if (event->bNotificationType == USB_CDC_NOTIFY_SPEED_CHANGE ||
+ event->bNotificationType == USB_CDC_NOTIFY_NETWORK_CONNECTION) {
+ pr_debug("Continue to mbim_do_notify()\n");
+ break;
+ } else {
+ pr_err("Unknown notification!!!\n");
+ BUG();
+ }
+ break;
+ case -ECONNRESET:
+ case -ESHUTDOWN:
+ /* connection gone */
+ pr_info("ESHUTDOWN/ECONNRESET, connection gone\n");
+ mbim->notify_state = MBIM_NOTIFY_NONE;
+ atomic_set(&mbim->online, 0);
+ atomic_set(&mbim->notify_count, 0);
+ atomic_set(&mbim->response_available, 0);
+ spin_unlock(&mbim->lock);
+ mbim_clear_queues(mbim);
+ mbim_reset_function_queue(mbim);
+ spin_lock(&mbim->lock);
+ break;
+ default:
+ pr_err("Unknown event %02x --> %d\n",
+ event->bNotificationType, req->status);
+ break;
+ }
+
+ mbim->notify_req = req;
+ mbim_do_notify(mbim);
+
+ spin_unlock(&mbim->lock);
+}
+
+static void mbim_ep0out_complete(struct usb_ep *ep, struct usb_request *req)
+{
+ /* now for SET_NTB_INPUT_SIZE only */
+ unsigned in_size = 0;
+ struct usb_function *f = req->context;
+ struct f_mbim *mbim = func_to_mbim(f);
+ struct mbim_ntb_input_size {
+ u32 ntb_input_size;
+ u16 ntb_max_datagrams;
+ u16 reserved;
+ } *ntb = NULL;
+ req->context = NULL;
+ if (req->status || req->actual != req->length) {
+ pr_err("Bad control-OUT transfer\n");
+ goto invalid;
+ }
+
+ if (req->length == 4) {
+ in_size = get_unaligned_le32(req->buf);
+ if (in_size < USB_CDC_NCM_NTB_MIN_IN_SIZE ||
+ in_size > mbim->ntb_input_size) {
+ pr_err("Illegal INPUT SIZE (%d) from host\n", in_size);
+ goto invalid;
+ }
+ } else if (req->length == 8) {
+ ntb = (struct mbim_ntb_input_size *)req->buf;
+ in_size = get_unaligned_le32(&(ntb->ntb_input_size));
+ if (in_size < USB_CDC_NCM_NTB_MIN_IN_SIZE ||
+ in_size > mbim->ntb_input_size) {
+ pr_err("Illegal INPUT SIZE (%d) from host\n", in_size);
+ goto invalid;
+ }
+ mbim->ntb_max_datagrams =
+ get_unaligned_le16(&(ntb->ntb_max_datagrams));
+ } else {
+ pr_err("Illegal NTB length %d\n", in_size);
+ goto invalid;
+ }
+
+ pr_debug("Set NTB INPUT SIZE %d\n", in_size);
+
+ mbim->ntb_input_size = in_size;
+ mbim->port.fixed_in_len = in_size;
+ return;
+
+invalid:
+ usb_ep_set_halt(ep);
+
+ pr_err("dev:%p Failed\n", mbim);
+
+ return;
+}
+
+static void
+fmbim_cmd_complete(struct usb_ep *ep, struct usb_request *req)
+{
+ struct f_mbim *dev = req->context;
+ struct ctrl_pkt *cpkt = NULL;
+ int len = req->actual;
+ struct usb_cdc_mbim_ctrl_msg *ctrl_msg = req->buf;
+
+ if (!dev) {
+ pr_err("mbim dev is null\n");
+ return;
+ }
+
+ if (req->status < 0) {
+ pr_err("mbim command error %d\n", req->status);
+ return;
+ }
+
+ spin_lock(&dev->lock);
+ cpkt = mbim_alloc_ctrl_pkt(len, GFP_ATOMIC);
+ if (!cpkt) {
+ pr_err("Unable to allocate ctrl pkt\n");
+ spin_unlock(&dev->lock);
+ return;
+ }
+
+ pr_debug("Add to cpkt_req_q packet with len = %d\n", len);
+ pr_debug("MessagType = %d, TransactionId = %d\n",
+ le32_to_cpu(ctrl_msg->dwMessagType),
+ le32_to_cpu(ctrl_msg->dwTransactionId));
+ cpkt->transaction_id = le32_to_cpu(ctrl_msg->dwTransactionId);
+ memcpy(cpkt->buf, req->buf, len);
+ list_add_tail(&cpkt->list, &dev->cpkt_req_q);
+ spin_unlock(&dev->lock);
+
+ /* wakeup read thread */
+ pr_debug("Wake up read queue\n");
+ wake_up(&dev->read_wq);
+
+ return;
+}
+
+static int
+mbim_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
+{
+ struct f_mbim *mbim = func_to_mbim(f);
+ struct usb_composite_dev *cdev = f->config->cdev;
+ struct usb_request *req = cdev->req;
+ struct ctrl_pkt *cpkt = NULL;
+ int value = -EOPNOTSUPP;
+ u16 w_index = le16_to_cpu(ctrl->wIndex);
+ u16 w_value = le16_to_cpu(ctrl->wValue);
+ u16 w_length = le16_to_cpu(ctrl->wLength);
+ int response_available = 0;
+
+ /*
+ * composite driver infrastructure handles everything except
+ * CDC class messages; interface activation uses set_alt().
+ */
+
+ /*
+ * For NCM+MBIM function we need to define and handle
+ * Microsoft OS specific strings/requests. This allows Win8
+ * to select correct driver (MBIM) for NCM+MBIM fallback mode
+ */
+ if (ctrl->bRequestType ==
+ (USB_DIR_IN | USB_TYPE_STANDARD | USB_RECIP_DEVICE)
+ && ctrl->bRequest == USB_REQ_GET_DESCRIPTOR
+ && (w_value >> 8) == USB_DT_STRING
+ && (w_value & 0xFF) == OS_STRING_ID) {
+
+ pr_debug("OS string request: %d index: %d value: %d length: %d\n",
+ ctrl->bRequest, w_index, w_value, w_length);
+
+ if(mbim->mode == MODE_NCM_MBIM) {
+ value = (w_length < sizeof(mbim_os_string)
+ ? w_length : sizeof(mbim_os_string));
+ memcpy(cdev->req->buf, mbim_os_string, value);
+ }
+
+ /* return here since composite.c will send for us */
+ return value;
+ }
+
+ /* Handle MBIM OS descriptor */
+ if ((ctrl->bRequestType & USB_TYPE_MASK) == USB_TYPE_VENDOR
+ && (ctrl->bRequest == USB_VENDOR_OSVC_DEFAULT)
+ && (ctrl->bRequestType & USB_DIR_IN)
+ && (w_index == MS_EXTENDED_COMPAT_ID
+ || w_index == MS_EXTENDED_PROPERTY)) {
+
+ pr_debug("vendor request: %d index: %d value: %d length: %d\n",
+ ctrl->bRequest, w_index, w_value, w_length);
+
+ if (mbim->mode == MODE_NCM_MBIM) {
+ value = (w_length < sizeof(mbim_ext_config_desc) ?
+ w_length : sizeof(mbim_ext_config_desc));
+ memcpy(cdev->req->buf, &mbim_ext_config_desc, value);
+ }
+
+ return value;
+ }
+
+ switch ((ctrl->bRequestType << 8) | ctrl->bRequest) {
+ case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_SET_ETHERNET_PACKET_FILTER:
+ /*
+ * see 6.2.30: no data, wIndex = interface,
+ * wValue = packet filter bitmap
+ */
+ if (w_length != 0 || w_index != mbim->ctrl_id)
+ goto invalid;
+ pr_info_ratelimited("packet filter %02x\n", w_value);
+ /*
+ * REVISIT locking of cdc_filter. This assumes the UDC
+ * driver won't have a concurrent packet TX irq running on
+ * another CPU; or that if it does, this write is atomic...
+ */
+ mbim->port.cdc_filter = w_value;
+ value = 0;
+ break;
+ /*
+ * and optionally:
+ * case USB_CDC_SEND_ENCAPSULATED_COMMAND:
+ * case USB_CDC_GET_ENCAPSULATED_RESPONSE:
+ * case USB_CDC_SET_ETHERNET_MULTICAST_FILTERS:
+ * case USB_CDC_SET_ETHERNET_PM_PATTERN_FILTER:
+ * case USB_CDC_GET_ETHERNET_PM_PATTERN_FILTER:
+ * case USB_CDC_GET_ETHERNET_STATISTIC:
+ */
+ case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_RESET_FUNCTION:
+
+ pr_info_ratelimited("USB_CDC_RESET_FUNCTION\n");
+ value = 0;
+ req->complete = fmbim_reset_cmd_complete;
+ req->context = mbim;
+ break;
+
+ case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_SEND_ENCAPSULATED_COMMAND:
+
+ pr_info_ratelimited("USB_CDC_SEND_ENCAPSULATED_COMMAND\n");
+
+ value = w_length;
+ req->complete = fmbim_cmd_complete;
+ req->context = mbim;
+ break;
+
+ case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_GET_ENCAPSULATED_RESPONSE:
+
+ pr_debug("USB_CDC_GET_ENCAPSULATED_RESPONSE\n");
+
+ if (w_value) {
+ pr_err("w_value > 0: %d\n", w_value);
+ break;
+ }
+
+ spin_lock(&mbim->lock);
+ if (list_empty(&mbim->cpkt_resp_q)) {
+ pr_err("ctrl resp queue empty\n");
+ spin_unlock(&mbim->lock);
+ break;
+ }
+
+ response_available = atomic_read(&mbim->response_available);
+
+ cpkt = list_first_entry(&mbim->cpkt_resp_q,
+ struct ctrl_pkt, list);
+ list_del(&cpkt->list);
+ spin_unlock(&mbim->lock);
+
+ value = min_t(unsigned, w_length, cpkt->len);
+ memcpy(req->buf, cpkt->buf, value);
+ pr_debug("cpkt=%p, transaction id=%d, \n", cpkt, cpkt->transaction_id);
+ mbim_free_ctrl_pkt(cpkt);
+
+ pr_info_ratelimited("copied encapsulated_response %d bytes\n",
+ value);
+ break;
+
+ case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_GET_NTB_PARAMETERS:
+
+ pr_info_ratelimited("USB_CDC_GET_NTB_PARAMETERS\n");
+
+ if (w_length == 0 || w_value != 0 || w_index != mbim->ctrl_id)
+ goto invalid;
+ value = w_length > sizeof ntb_parameters ?
+ sizeof ntb_parameters : w_length;
+ memcpy(req->buf, &ntb_parameters, value);
+ break;
+
+ case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_GET_NTB_INPUT_SIZE:
+
+ pr_info_ratelimited("USB_CDC_GET_NTB_INPUT_SIZE\n");
+
+ if (w_length < 4 || w_value != 0 || w_index != mbim->ctrl_id)
+ goto invalid;
+ put_unaligned_le32(mbim->ntb_input_size, req->buf);
+ value = 4;
+ pr_info_ratelimited("Reply to host INPUT SIZE %d\n",
+ mbim->ntb_input_size);
+ break;
+
+ case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_SET_NTB_INPUT_SIZE:
+
+ pr_info_ratelimited("USB_CDC_SET_NTB_INPUT_SIZE\n");
+
+ if (w_length != 4 && w_length != 8) {
+ pr_err("wrong NTB length %d\n", w_length);
+ goto invalid;;
+ }
+
+ if (w_value != 0 || w_index != mbim->ctrl_id)
+ goto invalid;;
+
+ req->complete = mbim_ep0out_complete;
+ req->length = w_length;
+ req->context = f;
+
+ value = req->length;
+ break;
+
+ case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_GET_NTB_FORMAT:
+ {
+ uint16_t format;
+
+ pr_info_ratelimited("USB_CDC_GET_NTB_FORMAT\n");
+
+ if (w_length < 2 || w_value != 0 || w_index != mbim->ctrl_id)
+ goto invalid;
+ format = (mbim->parser_opts == &ndp16_opts) ? 0x0000 : 0x0001;
+ put_unaligned_le16(format, req->buf);
+ value = 2;
+ pr_info_ratelimited("NTB FORMAT: sending %d\n", format);
+ break;
+ }
+
+ case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_SET_NTB_FORMAT:
+ {
+ pr_info_ratelimited("USB_CDC_SET_NTB_FORMAT ");
+
+ if (w_length != 0 || w_index != mbim->ctrl_id)
+ goto invalid;
+ switch (w_value) {
+ case 0x0000:
+ mbim->parser_opts = &ndp16_opts;
+ pr_info_ratelimited("MBIM16 selected\n");
+ break;
+ case 0x0001:
+ mbim->parser_opts = &ndp32_opts;
+ pr_info_ratelimited("MBIM32 selected\n");
+ break;
+ default:
+ goto invalid;
+ }
+ value = 0;
+ break;
+ }
+ case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_GET_CRC_MODE:
+ {
+ uint16_t is_crc;
+
+ if (w_length < 2 || w_value != 0 || w_index != mbim->ctrl_id)
+ goto invalid;
+ is_crc = mbim->is_crc ? 0x0001 : 0x0000;
+ put_unaligned_le16(is_crc, req->buf);
+ value = 2;
+ pr_debug("Host asked CRC MODE, sending %d\n", is_crc);
+ break;
+ }
+
+ case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8)
+ | USB_CDC_SET_CRC_MODE:
+ {
+ int ndp_hdr_crc = 0;
+
+ if (w_length != 0 || w_index != mbim->ctrl_id)
+ goto invalid;
+ switch (w_value) {
+ case 0x0000:
+ mbim->is_crc = false;
+ ndp_hdr_crc = NCM_NDP_HDR_NOCRC;
+ pr_debug("non-CRC mode selected\n");
+ break;
+ case 0x0001:
+ mbim->is_crc = true;
+ ndp_hdr_crc = NCM_NDP_HDR_CRC;
+ pr_debug("CRC mode selected\n");
+ break;
+ default:
+ goto invalid;
+ }
+ mbim->parser_opts->ndp_sign &= ~NCM_NDP_HDR_CRC_MASK;
+ mbim->parser_opts->ndp_sign |= ndp_hdr_crc;
+ value = 0;
+ break;
+ }
+
+ /* and disabled in mbim descriptor: */
+ /* case USB_CDC_GET_NET_ADDRESS: */
+ /* case USB_CDC_SET_NET_ADDRESS: */
+ /* case USB_CDC_GET_MAX_DATAGRAM_SIZE: */
+ /* case USB_CDC_SET_MAX_DATAGRAM_SIZE: */
+
+ default:
+invalid:
+ pr_err("invalid control req%02x.%02x v%04x i%04x l%d\n",
+ ctrl->bRequestType, ctrl->bRequest,
+ w_value, w_index, w_length);
+ }
+
+ /* respond with data transfer or status phase? */
+ if (value >= 0) {
+ pr_debug("control request: %02x.%02x v%04x i%04x l%d\n",
+ ctrl->bRequestType, ctrl->bRequest,
+ w_value, w_index, w_length);
+ req->zero = (value < w_length);
+ req->length = value;
+ value = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
+ if (value < 0) {
+ pr_err("queueing req failed: %02x.%02x, err %d\n",
+ ctrl->bRequestType,
+ ctrl->bRequest, value);
+ }
+#ifdef SERIAL_RESPONSE
+ if (response_available) {
+ pr_debug("Response available=%d (was busy before), try to send it again.\n", response_available);
+ atomic_dec(&mbim->response_available);
+ fmbim_ctrl_response_available(mbim);
+ }
+#endif
+ } else {
+ pr_err("ctrl req err %d: %02x.%02x v%04x i%04x l%d\n",
+ value, ctrl->bRequestType, ctrl->bRequest,
+ w_value, w_index, w_length);
+ }
+
+ /* device either stalls (value < 0) or reports success */
+ return value;
+}
+
+static int mbim_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
+{
+ int ret;
+ struct f_mbim *mbim = func_to_mbim(f);
+ struct usb_composite_dev *cdev = f->config->cdev;
+
+ /* Control interface has only altsetting 0 */
+ if (intf == mbim->ctrl_id) {
+
+ pr_info("CONTROL_INTERFACE: alt = %d\n", alt);
+
+ if ((mbim->mode != MODE_NCM_MBIM && alt != 0) ||
+ (mbim->mode == MODE_NCM_MBIM && alt > 1))
+ goto fail;
+
+ if (mbim->notify->driver_data) {
+ pr_info("reset mbim control %d\n", intf);
+ usb_ep_disable(mbim->notify);
+ }
+
+ if (mbim->mode == MODE_NCM || (mbim->mode == MODE_NCM_MBIM && alt == 0))
+ mbim->state = STATE_NCM;
+ else if (mbim->mode == MODE_MBIM || (mbim->mode == MODE_NCM_MBIM && alt == 1))
+ mbim->state = STATE_MBIM;
+ else
+ BUG();
+
+ if (!(mbim->notify->desc)) {
+ DBG(cdev, "init mbim ctrl %d\n", intf);
+ if (config_ep_by_speed(cdev->gadget, f, mbim->notify))
+ goto fail;
+ }
+
+ usb_ep_enable(mbim->notify);
+ mbim->notify->driver_data = mbim;
+
+ /* Data interface has up to 3 altsettings, 0-2 */
+ } else if (intf == mbim->data_id) {
+
+ pr_info("DATA_INTERFACE: alt = %d\n", alt);
+
+ if ((mbim->mode != MODE_NCM_MBIM && alt > 1) ||
+ (mbim->mode == MODE_NCM_MBIM && alt > 2))
+ goto fail;
+
+ if (mbim->port.in_ep->driver_data) {
+ pr_info("reset mbim\n");
+ /* notify req is still queued on usb controller
+ * suppose this should return the req to mbim->notify_req
+ */
+ if (unlikely(NULL == mbim->notify_req)) {
+ ret = usb_ep_dequeue(mbim->notify, g_notify_req);
+ pr_info("dequeue notify_req done ret: %d\n", ret);
+ }
+
+ mbim_reset_values(mbim);
+ mbim_clear_queues(mbim);
+ mbim_reset_function_queue(mbim);
+ gether_disconnect(&mbim->port);
+ }
+
+ /*
+ * CDC Network only sends data in non-default altsettings.
+ * Changing altsettings resets filters, statistics, etc.
+ */
+ if (alt == 1 || alt == 2) {
+ struct net_device *net;
+ pr_info("Alt set %d, initialize ports\n", alt);
+
+ if (!mbim->port.in_ep->desc ||
+ !mbim->port.out_ep->desc) {
+ pr_info("init mbim\n");
+ if (config_ep_by_speed(cdev->gadget, f,
+ mbim->port.in_ep) ||
+ config_ep_by_speed(cdev->gadget, f,
+ mbim->port.out_ep)) {
+ mbim->port.in_ep->desc = NULL;
+ mbim->port.out_ep->desc = NULL;
+ goto fail;
+ }
+ }
+
+ if (mbim->state == STATE_NCM) {
+ mbim->port.open = mbim_net_open;
+ mbim->port.close = mbim_net_close;
+#ifdef CONFIG_USB_G_NCM_MULT_PKT_SUPPORT
+ mbim->port.wrap = ncm_wrap_ntb_multipkt;
+#else
+ mbim->port.wrap = ncm_wrap_ntb;
+#endif
+ mbim->port.unwrap = ncm_unwrap_ntb;
+ mbim->port.unwrap_fixup = NULL;
+ } else { /* STATE_MBIM */
+ /* Ruslan: originally was commented out in this driver*/
+ mbim->port.open = NULL;
+ mbim->port.close = NULL;
+
+#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
+ mbim->port.wrap = mbim_wrap_ntb_multipkt;
+#ifdef CONFIG_USBNET_USE_SG
+ mbim->port.is_sg_mode = 1;
+#endif
+#else
+ mbim->port.wrap = mbim_wrap_ntb;
+#endif
+ mbim->port.unwrap = mbim_unwrap_ntb;
+#ifndef CONFIG_ASR_TOE
+ mbim->port.unwrap_fixup = mbim_process_dgram;
+#endif
+ }
+ /* TODO */
+ /* Enable zlps by default for MBIM conformance;
+ * override for musb_hdrc (avoids txdma ovhead)
+ */
+ mbim->port.is_zlp_ok = !(
+ gadget_is_musbhdrc(cdev->gadget)
+ );
+ mbim->port.cdc_filter = DEFAULT_FILTER;
+
+ atomic_set(&mbim->online, 1);
+
+ spin_lock(&mbim->lock);
+ if (mbim->is_open)
+ mbim_notify(mbim);
+ spin_unlock(&mbim->lock);
+
+#ifdef CONFIG_ASR_TOE
+ if (mbim->state == STATE_NCM) {
+ mbim->port.ueth_type = UETHER_NCM;
+ } else { /* STATE_MBIM */
+ mbim->port.ueth_type = UETHER_MBIM;
+ }
+#endif
+ pr_info("activate mbim\n");
+ net = gether_connect(&mbim->port);
+ if (IS_ERR(net))
+ return PTR_ERR(net);
+ }
+ } else
+ goto fail;
+
+ /* wakeup file threads */
+ wake_up(&mbim->open_wq);
+ wake_up(&mbim->read_wq);
+ wake_up(&mbim->write_wq);
+
+ return 0;
+
+fail:
+ pr_err("ERROR: Illegal Interface\n");
+ return -EINVAL;
+}
+
+/*
+ * Because the data interface supports multiple altsettings,
+ * this MBIM function *MUST* implement a get_alt() method.
+ */
+static int mbim_get_alt(struct usb_function *f, unsigned intf)
+{
+ struct f_mbim *mbim = func_to_mbim(f);
+
+ if (intf == mbim->ctrl_id)
+ return 0;
+ return mbim->port.in_ep->driver_data ? 1 : 0;
+}
+
+#define MBIM_SK_CB_SIGN (0x4D49424D) /* MBIM */
+
+struct mbim_cb {
+ u32 sign;
+ u16 tci;
+};
+
+#define MBIM_SKB_CB(skb) ((struct mbim_cb *)((skb)->cb))
+struct nth {
+ __le32 dwSignature;
+ __le16 wHeaderLength;
+ __le16 wSequence;
+ union {
+ struct {
+ __le16 wBlockLength;
+ __le16 wFpIndex;
+ } nth16 __attribute__ ((packed));
+ struct {
+ __le32 wBlockLength;
+ __le32 wFpIndex;
+ } nth32 __attribute__ ((packed));
+ } nth_type;
+} __attribute__ ((packed));
+
+struct ndp {
+ __le32 dwSignature;
+ __le16 wLength;
+ union {
+ struct {
+ __le16 wNextFpIndex;
+ } ndp16 __attribute__ ((packed));
+ struct {
+ __le16 wReserved6;
+ __le32 wNextFpIndex;
+ __le32 wReserved12;
+ } ndp32 __attribute__ ((packed));
+ } ndp_type;
+} __attribute__ ((packed));
+
+
+#define NCM_16BIT_SIGN (0x484d434e)
+
+#define NTH_FIELD(nth_header, field) \
+ (mbim->parser_opts->nth_sign == NCM_16BIT_SIGN ? \
+ (void *)&(nth_header->nth_type.nth16.field) : \
+ (void *)&(nth_header->nth_type.nth32.field))
+
+#define NDP_FIELD(ndp_header, field) \
+ (mbim->parser_opts->nth_sign == NCM_16BIT_SIGN ? \
+ (void *)&(ndp_header->ndp_type.ndp16.field) : \
+ (void *)&(ndp_header->ndp_type.ndp32.field))
+
+#define NTB_MAX_HEADER_SIZE(skb) (*(unsigned int *)skb->cb)
+
+#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
+#ifndef CONFIG_USBNET_USE_SG
+#define MBIM_NOT_USE_SG
+#endif
+#endif
+
+#ifndef CONFIG_ASR_TOE
+#if (defined CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS) || (defined MBIM_NOT_USE_SG)
+static struct sk_buff *add_ndp_header(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx, __le32 sign)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ struct sk_buff *first_skb, *prev_skb;
+ int ncb_len;
+ struct ndp *ndp_header;
+ __le16 *tmp;
+ __le16 offset = 0;
+ int ndp_len = 0, ndp_align = ntb_parameters.wNdpInAlignment;
+ int div = ntb_parameters.wNdpInDivisor;
+ int rem = ntb_parameters.wNdpInPayloadRemainder;
+ int pad = 0;
+ int ndp_pad;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+
+ ndp_len = opts->ndp_size;
+ ndp_len += 2 * 2 * opts->dgram_item_len; /* Datagram entry */
+ ndp_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
+
+ first_skb = skb_peek(&aggr_ctx->skb_list);
+ prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
+
+ BUG_ON(skb_queue_empty(&aggr_ctx->skb_list));
+ BUG_ON(!first_skb);
+ BUG_ON(!prev_skb);
+ BUG_ON(!aggr_ctx->total_size);
+
+ ndp_header = (void *)skb_push(skb, ndp_len);
+ memset(ndp_header, 0, ndp_len);
+
+ /* NDP */
+ put_unaligned_le32(sign, &ndp_header->dwSignature);
+ put_unaligned_le16(ndp_len, &ndp_header->wLength);
+
+ tmp = (void *)ndp_header + opts->ndp_size;
+ /* (d)wDatagramIndex[0] */
+ put_mbim(&tmp, opts->dgram_item_len, skb->data + ndp_len - first_skb->data);
+ /* (d)wDatagramLength[0] */
+ put_mbim(&tmp, opts->dgram_item_len, skb->len - ndp_len);
+ /* (d)wDatagramIndex[1] and (d)wDatagramLength[1] already zeroed */
+ if (unlikely(mbim->is_crc)) {
+ if (mbim->mode == MODE_MBIM) {
+ pr_err("MBIM doesn't support CRC!!\n");
+ } else{
+ pr_err("NCM fast-path doesn't support CRC in multiple packets!!"
+ "CI needs to allocate additional space for CRC at the end of the packet.\n");
+ }
+ BUG();
+ }
+
+ /* update prev NDP header with new NDP index */
+ if (first_skb == prev_skb) {
+ ncb_len = opts->nth_size;
+ ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
+ ncb_len += ndp_pad;
+ pad = ALIGN(ncb_len, div) + rem - ncb_len;
+ ncb_len += pad;
+ offset = ncb_len;
+ }
+ ndp_header = (void *)(prev_skb->data + offset);
+ put_mbim_no_inc(NDP_FIELD(ndp_header, wNextFpIndex), opts->next_fp_index, skb->data - first_skb->data);
+ return skb;
+}
+#endif
+#endif
+
+
+static struct sk_buff *mbim_wrap_ntb(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ struct sk_buff *skb2;
+ int ncb_len = 0;
+ int headroom;
+ __le16 *tmp;
+ int div = ntb_parameters.wNdpInDivisor;
+ int rem = ntb_parameters.wNdpInPayloadRemainder;
+ int pad;
+ struct nth *nth_header;
+ struct ndp *ndp_header;
+ int ndp_align = ntb_parameters.wNdpInAlignment;
+ int ndp_pad;
+ unsigned max_size = mbim->port.fixed_in_len;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
+ u16 tci = 0;
+ u8 *c = NULL;
+ __le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
+ struct net_device *net_dev = port->ioport->net;
+
+ BUG_ON(!skb);
+ if (skb->len <= ETH_HLEN)
+ goto error;
+
+ /* Reply for outgoing ARP request cause MBIM HOST will NOT answer! */
+ if (((struct ethhdr *)skb->data)->h_proto == htons(ETH_P_ARP)) {
+ struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
+ if (arp->ar_op == htons(ARPOP_REQUEST)) {
+ unsigned char * arp_ptr = (unsigned char *)(arp + 1);
+ __be32 dest_ip, src_ip;
+ pr_info_ratelimited("Reply for outgoing ARP request cause MBIM HOST will NOT answer\n");
+ arp_ptr += ETH_ALEN;
+ memcpy(&dest_ip, arp_ptr, 4);
+ arp_ptr += 4;
+ arp_ptr += ETH_ALEN;
+ memcpy(&src_ip, arp_ptr, 4);
+ skb2 = arp_create(ARPOP_REPLY, ETH_P_ARP, dest_ip, net_dev, src_ip,
+ net_dev->dev_addr, port->ioport->host_mac, net_dev->dev_addr);
+ BUG_ON(!skb2);
+ dev_kfree_skb_any(skb);
+
+ skb2->protocol = eth_type_trans(skb2, net_dev);
+ net_dev->stats.rx_packets++;
+ net_dev->stats.rx_bytes += skb2->len;
+
+ if (netif_rx(skb2) != NET_RX_SUCCESS)
+ pr_err("Faild to send ARP response to myself\n");
+ /* This ARP request will be dropped (tx_dropped++) */
+ return NULL;
+ }
+ }
+
+ /* mapping VLANs to MBIM sessions:
+ * no tag => IPS session <0>
+ * 1 - 255 => IPS session <vlanid>
+ * 256 - 511 => DSS session <vlanid - 256>
+ * 512 - 4095 => unsupported, drop
+ */
+ if (vlan_get_tag(skb, &tci) >= 0 ) {
+ switch (tci & 0x0f00) {
+ case 0x0000: /* VLAN ID 0 - 255 */
+ break;
+ case 0x0100: /* VLAN ID 256 - 511 */
+ sign = cpu_to_le32(USB_CDC_MBIM_NDP16_DSS_SIGN);
+ break;
+ default:
+ pr_err("unsupported tci=0x%04x\n", tci);
+ goto error;
+ }
+
+ c = (u8 *)&sign;
+ c[3] = tci;
+ skb_pull(skb, ETH_HLEN + VLAN_HLEN);
+ pr_debug("VLAN tci=0x%04x\n", tci);
+ } else {
+ skb_pull(skb, ETH_HLEN);
+ }
+
+ ncb_len += opts->nth_size;
+ ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
+ ncb_len += ndp_pad;
+ ncb_len += opts->ndp_size;
+ ncb_len += 2 * 2 * opts->dgram_item_len; /* Datagram entry */
+ ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
+ pad = ALIGN(ncb_len, div) + rem - ncb_len;
+ ncb_len += pad;
+
+ if (ncb_len + skb->len + crc_len > max_size)
+ goto error;
+
+ if (unlikely(skb->cloned || (skb_headroom(skb) < ncb_len)
+ || (skb_tailroom(skb) < crc_len))) {
+ skb2 = skb_copy_expand(skb, ncb_len, crc_len, GFP_ATOMIC);
+ BUG_ON(!skb2);
+ dev_kfree_skb_any(skb);
+ skb = skb2;
+ }
+
+ headroom = ncb_len;
+ // NTH
+ nth_header = (void *) skb_push(skb, headroom);
+ memset(nth_header, 0, headroom);
+
+ put_unaligned_le32(opts->nth_sign, &nth_header->dwSignature); // dwSignature
+ put_unaligned_le16(opts->nth_size, &nth_header->wHeaderLength); // wHeaderLength
+ // skip wSequence
+ put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->len); //(d)wBlockLength
+ // the first pointer NTH_FIELD(nth_header, wFpIndex) is right after the NTH + align
+ put_mbim_no_inc(NTH_FIELD(nth_header, wFpIndex), opts->fp_index, opts->nth_size + ndp_pad); //wNdpIndex(wFpIndex)
+
+ // NDP
+ ndp_header = (void*)nth_header + opts->nth_size + ndp_pad;
+ put_unaligned_le32(sign, &ndp_header->dwSignature); // dwSignature
+ put_unaligned_le16(ncb_len - opts->nth_size - pad, &ndp_header->wLength); // wLength
+
+ // skip reserved (d)wNextNdpIndex ((d)wNextFpIndex)
+ tmp += opts->reserved1;
+ tmp += opts->next_fp_index;
+ tmp += opts->reserved2;
+
+ if (mbim->is_crc) {
+ pr_err("MBIM doesn't support CRC!!\n");
+ BUG();
+ }
+
+ tmp = (void *)ndp_header + opts->ndp_size;
+ put_mbim(&tmp, opts->dgram_item_len, headroom); // (d)wDatagramIndex[0]
+ put_mbim(&tmp, opts->dgram_item_len, skb->len - headroom); // (d)wDatagramLength[0]
+ // (d)wDatagramIndex[1] and (d)wDatagramLength[1] already zeroed
+
+ return skb;
+error:
+ if (skb)
+ dev_kfree_skb_any(skb);
+ return NULL;
+}
+
+#ifdef CONFIG_ASR_TOE
+static struct sk_buff *mbim_wrap_ntb_multipkt(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
+{
+ struct sk_buff *first_skb, *prev_skb;
+
+ BUG_ON(!aggr_ctx);
+ BUG_ON(!skb);
+
+ if (skb_queue_empty(&aggr_ctx->skb_list) && list_empty(&aggr_ctx->toe_list)) {
+ skb = mbim_wrap_ntb(port, skb, aggr_ctx);
+ if (likely(skb)) {
+ aggr_ctx->total_size = skb->len;
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+ }
+ aggr_ctx->is_toe = false;
+ return skb;
+ }
+
+ if (aggr_ctx->is_toe) {
+ aggr_ctx->pending_skb = skb;
+ first_skb = (struct sk_buff *)list_first_entry(&aggr_ctx->toe_list, struct toe_pkt_desc, list);
+ return first_skb;
+ }
+
+ first_skb = skb_peek(&aggr_ctx->skb_list);
+ prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
+
+ BUG_ON(!first_skb);
+ BUG_ON(!prev_skb);
+ BUG_ON(first_skb == prev_skb && 1 != skb_queue_len(&aggr_ctx->skb_list));
+ BUG_ON(!aggr_ctx->total_size);
+
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+}
+
+static void mbim_add_eth_header(struct gether *port, struct sk_buff *skb)
+{
+ struct ethhdr *header;
+ struct net_device *dev = port->ioport->net;
+ __be16 proto = htons(ETH_P_802_3);
+
+ if (!skb)
+ return;
+
+ if (MBIM_SKB_CB(skb)->tci < 256) { /* IPS session? */
+ switch (*(skb->data) & 0xf0) {
+ case 0x40:
+ proto = htons(ETH_P_IP);
+ break;
+ case 0x60:
+#if 0
+ if (is_neigh_solicit(buf, len))
+ do_neigh_solicit(dev, buf, tci);
+#endif
+ proto = htons(ETH_P_IPV6);
+ break;
+ default:
+ return;
+ }
+ }
+
+ if (skb_headroom(skb) < sizeof(*header))
+ {
+ pr_err("%s: Headroom is not enough to add eth header\n",__func__);
+ return;
+ }
+ header = (void *)skb_push(skb,sizeof(*header));
+ memset(header, 0, sizeof(*header));
+
+ header->h_proto = proto;
+ memcpy(header->h_source, port->ioport->host_mac, ETH_ALEN);
+ memcpy(header->h_dest, dev->dev_addr, ETH_ALEN);
+#if 0
+ /* map MBIM session to VLAN */
+ if (tci)
+ vlan_put_tag(skb, htons(ETH_P_8021Q), tci);
+#endif
+ v7_dma_flush_range((void *)header, (void *)((u32)header + sizeof(*header)));
+}
+
+#ifdef CONFIG_MBIM_INPUT_AS_RNDIS
+static void mbim_add_rndis_header(struct sk_buff *skb)
+{
+ struct rndis_packet_msg_type *header;
+
+ if (!skb)
+ return;
+ if (skb_headroom(skb) < sizeof(*header))
+ {
+ pr_err("%s: Headroom is not enough to add rndis header\n",__func__);
+ return;
+ }
+ header = (void *)skb_push(skb, sizeof(*header));
+
+ memset(header, 0, sizeof *header);
+ header->MessageType = cpu_to_le32(RNDIS_MSG_PACKET);
+ header->MessageLength = cpu_to_le32(skb->len);
+ header->DataOffset = cpu_to_le32(36);
+ header->DataLength = cpu_to_le32(skb->len - sizeof(*header));
+ v7_dma_flush_range((void *)header, (void *)((u32)header + sizeof(*header)));
+}
+#endif
+
+#define MBIM_SKB_HEADROOM_RESERVE (64)
+#define MBIM_SKB_COPY_ALLOC_LENGTH (1514 + MBIM_SKB_HEADROOM_RESERVE + NET_IP_ALIGN)
+static struct sk_buff *mbim_copy_skb(struct sk_buff *skb, unsigned offset, unsigned len)
+{
+ struct sk_buff *skb2;
+
+ skb2 = bm_usb_alloc_skb(MBIM_SKB_COPY_ALLOC_LENGTH, GFP_ATOMIC);
+ if (!skb2)
+ {
+ pr_info("%s: No memory, dropping packet.\n",__func__);
+ return NULL;
+ }
+ skb_reserve(skb2, MBIM_SKB_HEADROOM_RESERVE);
+ memcpy(skb_put(skb2, len), (skb->data + offset), len);
+
+ return skb2;
+}
+
+static int mbim_unwrap_ntb(struct gether *port,
+ struct sk_buff *skb,
+ struct sk_buff_head *list)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ __le16 *tmp = (void *) skb->data;
+ unsigned index, index2;
+ unsigned dg_len, dg_len2;
+ unsigned ndp_len;
+ struct sk_buff *skb2 = NULL;
+ struct net_device *net_dev = port->ioport->net;
+ int ret = -EINVAL;
+ unsigned max_size = mbim->port.fixed_out_len;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ int dgram_counter;
+ u16 tci = 0;
+ u8 *c = NULL;
+
+ /* dwSignature */
+ if (get_unaligned_le32(tmp) != opts->nth_sign) {
+ pr_info("Wrong NTH SIGN, skblen %d\n",
+ skb->len);
+ print_hex_dump(KERN_INFO, "HEAD:\n", DUMP_PREFIX_ADDRESS, 32, 1,
+ skb->data, 32, false);
+ goto err;
+ }
+
+ tmp += 2;
+ /* wHeaderLength */
+ if (get_unaligned_le16(tmp++) != opts->nth_size) {
+ pr_info("Wrong NTB headersize\n");
+ goto err;
+ }
+
+ tmp++; /* skip wSequence */
+ /* (d)wBlockLength */
+ if (get_mbim(&tmp, opts->block_length) > max_size) {
+ pr_info("OUT size exceeded\n");
+ goto err;
+ }
+
+ index = get_mbim(&tmp, opts->fp_index);
+ /* MBIM 3.2 */
+ if (((index % 4) != 0) && (index < opts->nth_size)) {
+ pr_info("Bad index: %x\n",
+ index);
+ goto err;
+ }
+
+ /* walk through NDP */
+ tmp = ((void *)skb->data) + index;
+ switch (get_unaligned_le32(tmp) & cpu_to_le32(0x00ffffff)) {
+ case USB_CDC_MBIM_NDP16_IPS_SIGN:
+ case USB_CDC_MBIM_NDP32_IPS_SIGN:
+ c = (u8 *)tmp;
+ tci = c[3];
+ break;
+ case USB_CDC_MBIM_NDP16_DSS_SIGN:
+ case USB_CDC_MBIM_NDP32_DSS_SIGN:
+ c = (u8 *)tmp;
+ tci = c[3] + 256;
+ break;
+ default:
+ pr_info("unsupported NDP signature <0x%08x>\n", le32_to_cpu(tmp));
+ goto err;
+ }
+
+ tmp += 2;
+ ndp_len = get_unaligned_le16(tmp++);
+ /*
+ * MBIM 3.3.1
+ * entry is 2 items
+ * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
+ * minimal: struct usb_cdc_mbim_ndpX + normal entry + zero entry
+ */
+ if ((ndp_len < opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2))
+ || (ndp_len % opts->ndplen_align != 0)) {
+ pr_info("Bad NDP length: %x\n", ndp_len);
+ goto err;
+ }
+ tmp += opts->reserved1;
+ tmp += opts->next_fp_index; /* skip reserved (d)wNextFpIndex */
+ tmp += opts->reserved2;
+
+ ndp_len -= opts->ndp_size;
+ index2 = get_mbim(&tmp, opts->dgram_item_len);
+ dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
+ dgram_counter = 0;
+
+ do {
+ index = index2;
+ dg_len = dg_len2;
+
+ if (mbim->is_crc) {
+ pr_err("MBIM doesn't support CRC!!\n");
+ BUG();
+ }
+ index2 = get_mbim(&tmp, opts->dgram_item_len);
+ dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
+
+ if (index2 == 0 || dg_len2 == 0) {
+ skb2 = skb;
+ } else {
+ skb2 = mbim_copy_skb(skb, index, dg_len);
+ if (skb2 == NULL) {
+ goto err;
+ } else {
+ goto skb_nopull;
+ }
+ }
+
+ if (!skb_pull(skb2, index)) {
+ ret = -EOVERFLOW;
+ goto err;
+ }
+
+skb_nopull:
+ skb_trim(skb2, dg_len);
+
+ skb2->dev = net_dev;
+ MBIM_SKB_CB(skb2)->sign = MBIM_SK_CB_SIGN;
+ MBIM_SKB_CB(skb2)->tci = tci;
+
+ BUG_ON(tci > 255); //Fix this
+ /* map MBIM session to VLAN */
+ if (tci) {//maybe do it always, also for VLAN ID 0
+ pr_err("Adding VLAN\n");
+ BUG();
+ vlan_put_tag(skb2, htons(ETH_P_8021Q), tci);
+ }
+
+ mbim_add_eth_header(port, skb2);
+#ifdef CONFIG_MBIM_INPUT_AS_RNDIS
+ mbim_add_rndis_header(skb2);
+#endif
+ skb_queue_tail(list, skb2);
+ ndp_len -= 2 * (opts->dgram_item_len * 2);
+ dgram_counter++;
+ if (index2 == 0 || dg_len2 == 0)
+ break;
+ } while (ndp_len > 2 * (opts->dgram_item_len * 2)); /* zero entry */
+ return 0;
+err:
+ skb_queue_purge(list);
+ dev_kfree_skb_any(skb);
+ return ret;
+}
+
+#else /* CONFIG_ASR_TOE */
+#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
+#ifdef CONFIG_USBNET_USE_SG
+static struct sk_buff *mbim_wrap_ntb_multipkt(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ int aggregate_length, skb_qlen;
+ struct nth *nth_header;
+ struct ndp *ndp_header;
+ int div = ntb_parameters.wNdpInDivisor;
+ unsigned max_size = mbim->port.fixed_in_len;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ int skb_len;
+ int reserve_headroom_size;
+ int single_entry_ndp_size = opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2);
+ struct sk_buff *first_skb, *prev_skb;
+ u16 tci = 0;
+ u8 *c = NULL;
+ __le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
+ __le16 *tmp;
+ int ndp_len;
+
+ BUG_ON(!aggr_ctx);
+ BUG_ON(!skb);
+
+ skb_qlen = skb_queue_len(&aggr_ctx->skb_list);
+
+ if (skb_queue_empty(&aggr_ctx->skb_list)) {
+ skb = mbim_wrap_ntb(port,skb,aggr_ctx);
+ if (likely(skb)) {
+ aggr_ctx->total_size = skb->len;
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+ aggr_ctx->num_sgs = 1;
+ } else {
+ aggr_ctx->total_size = 0;
+ aggr_ctx->num_sgs = 0;
+ pr_err("error: %s 1st pkt failed\n", __func__);
+ }
+ return skb;
+ }
+
+ first_skb = skb_peek(&aggr_ctx->skb_list);
+ prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
+
+ BUG_ON(!first_skb);
+ BUG_ON(!prev_skb);
+ BUG_ON(first_skb == prev_skb && 1!=skb_queue_len(&aggr_ctx->skb_list));
+ BUG_ON(!aggr_ctx->total_size);
+
+ /* Reply for outgoing ARP request cause MBIM HOST will NOT answer! */
+ if (((struct ethhdr *)skb->data)->h_proto == htons(ETH_P_ARP)) {
+ struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
+ if (arp->ar_op == htons(ARPOP_REQUEST)) {
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+ }
+ }
+
+ skb_len = skb->len;
+ if (vlan_get_tag(skb, &tci) >= 0 ) {
+ skb_len = skb_len - ETH_HLEN - VLAN_HLEN;
+ reserve_headroom_size = skb_headroom(skb) + ETH_HLEN + VLAN_HLEN;
+ } else {
+ skb_len = skb_len - ETH_HLEN;
+ reserve_headroom_size = skb_headroom(skb) + ETH_HLEN;
+ }
+
+ skb_len += single_entry_ndp_size;
+ aggregate_length = aggr_ctx->total_size + skb_len;
+
+ if((ALIGN(skb_len, div) - skb_len) > 0 ||
+ (ALIGN(prev_skb->len, div) - prev_skb->len) > 0) {
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+ }
+
+ /* mbim aggregation condition */
+ if (aggregate_length > max_size ||
+ (mbim->ntb_max_datagrams != 0 && mbim->ntb_max_datagrams <= skb_qlen) ||
+ skb_qlen >= MBIM_MAX_DGRAMS_AGGREGATION ||
+ skb_cloned(skb) ||
+ reserve_headroom_size < single_entry_ndp_size ||
+ aggr_ctx->num_sgs >= (USBNET_SG_NENTS - 1)) {
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+ }
+
+ /* mapping VLANs to MBIM sessions:
+ * no tag => IPS session <0>
+ * 1 - 255 => IPS session <vlanid>
+ * 256 - 511 => DSS session <vlanid - 256>
+ * 512 - 4095 => unsupported, drop
+ */
+ if (vlan_get_tag(skb, &tci) >= 0 ) {
+ switch (tci & 0x0f00) {
+ case 0x0000: /* VLAN ID 0 - 255 */
+ break;
+ case 0x0100: /* VLAN ID 256 - 511 */
+ sign = cpu_to_le32(USB_CDC_MBIM_NDP16_DSS_SIGN);
+ break;
+ default:
+ pr_err("unsupported tci=0x%04x\n", tci);
+ goto error;
+ }
+ c = (u8 *)&sign;
+ c[3] = tci;
+ skb_pull(skb, ETH_HLEN + VLAN_HLEN);
+ pr_debug("VLAN tci=0x%04x\n", tci);
+ } else {
+ skb_pull(skb, ETH_HLEN);
+ }
+
+ // update NDP
+ ndp_len = opts->ndp_size;
+ ndp_len += 2 * 2 * opts->dgram_item_len; /* Datagram entry */
+ ndp_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
+ ndp_header = (void *)skb_push(skb, ndp_len);
+ memset(ndp_header, 0, ndp_len);
+
+ // dwSignature and wLength
+ put_unaligned_le32(sign, &ndp_header->dwSignature);
+ put_unaligned_le16(ndp_len, &ndp_header->wLength);
+
+ tmp = (void *)ndp_header + opts->ndp_size;
+ // (d)wDatagramIndex[0] and (d)wDatagramLength[0]
+ put_mbim(&tmp, opts->dgram_item_len, aggr_ctx->total_size + ndp_len);
+ put_mbim(&tmp, opts->dgram_item_len, skb->len - ndp_len);
+ /* (d)wDatagramIndex[1] and (d)wDatagramLength[1] already zeroed */
+ if (unlikely(mbim->is_crc)) {
+ pr_err("MBIM doesn't support CRC!!\n");
+ BUG();
+ }
+
+ // update wNextFpIndex(wNextNdpIndex) of prev ntb
+ if (first_skb == prev_skb) {
+ ndp_header = (void *)prev_skb->data + opts->nth_size;
+ } else {
+ ndp_header = (void *)prev_skb->data;
+ }
+ put_mbim_no_inc(NDP_FIELD(ndp_header, wNextFpIndex), opts->next_fp_index, aggr_ctx->total_size);
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+
+ aggr_ctx->total_size += skb->len;
+ aggr_ctx->num_sgs++;
+
+ // update NTH
+ nth_header = (void *)first_skb->data;
+ // (d)wBlockLength
+ put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, aggr_ctx->total_size);
+
+ return first_skb;
+
+error:
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+}
+#else
+static struct sk_buff *mbim_wrap_ntb_multipkt(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ int padding, aggregate_length, skb_qlen;
+ struct nth *nth_header;
+ unsigned max_size = mbim->port.fixed_in_len;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ int single_entry_ndp_size = opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2);
+ struct sk_buff *first_skb, *prev_skb;
+ u16 tci = 0;
+ u8 *c = NULL;
+ __le32 sign = cpu_to_le32(USB_CDC_MBIM_NDP16_IPS_SIGN);
+
+ BUG_ON(!aggr_ctx);
+ BUG_ON(!skb);
+
+ skb_qlen = skb_queue_len(&aggr_ctx->skb_list);
+
+ if (skb_queue_empty(&aggr_ctx->skb_list)) {
+ skb = mbim_wrap_ntb(port,skb,aggr_ctx);
+ if (likely(skb)) {
+ aggr_ctx->total_size = skb->len;
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+ }
+ return skb;
+ }
+
+ first_skb = skb_peek(&aggr_ctx->skb_list);
+ prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
+
+ BUG_ON(!first_skb);
+ BUG_ON(!prev_skb);
+ BUG_ON(first_skb == prev_skb && 1!=skb_queue_len(&aggr_ctx->skb_list));
+ BUG_ON(!aggr_ctx->total_size);
+
+ /* Reply for outgoing ARP request cause MBIM HOST will NOT answer! */
+ if (((struct ethhdr *)skb->data)->h_proto == htons(ETH_P_ARP)) {
+ struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
+ if (arp->ar_op == htons(ARPOP_REQUEST)) {
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+ }
+ }
+
+ padding = (unsigned)skb->data - single_entry_ndp_size - (unsigned)prev_skb->tail;
+ aggregate_length = aggr_ctx->total_size + skb->len + padding + single_entry_ndp_size;
+ BUG_ON(aggregate_length != skb->tail - first_skb->data);
+
+ if (vlan_get_tag(skb, &tci) >= 0 ) {
+ padding = padding + ETH_HLEN + VLAN_HLEN;
+ } else {
+ padding = padding + ETH_HLEN;
+ }
+
+ /* mbim aggregation condition */
+ if (aggregate_length > max_size ||
+ (mbim->ntb_max_datagrams != 0 && mbim->ntb_max_datagrams <= skb_qlen) ||
+ skb_qlen >= MBIM_MAX_DGRAMS_AGGREGATION ||
+ skb_cloned(skb) ||
+ skb_headroom(skb) < single_entry_ndp_size ||
+ padding > AGGR_MAX_PADDING ||
+ padding < 0) {
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+ }
+
+ /* mapping VLANs to MBIM sessions:
+ * no tag => IPS session <0>
+ * 1 - 255 => IPS session <vlanid>
+ * 256 - 511 => DSS session <vlanid - 256>
+ * 512 - 4095 => unsupported, drop
+ */
+ if (vlan_get_tag(skb, &tci) >= 0 ) {
+ switch (tci & 0x0f00) {
+ case 0x0000: /* VLAN ID 0 - 255 */
+ break;
+ case 0x0100: /* VLAN ID 256 - 511 */
+ sign = cpu_to_le32(USB_CDC_MBIM_NDP16_DSS_SIGN);
+ break;
+ default:
+ pr_err("unsupported tci=0x%04x\n", tci);
+ goto error;
+ }
+ c = (u8 *)&sign;
+ c[3] = tci;
+ skb_pull(skb, ETH_HLEN + VLAN_HLEN);
+ pr_debug("VLAN tci=0x%04x\n", tci);
+ } else {
+ skb_pull(skb, ETH_HLEN);
+ }
+
+ // update NTH
+ nth_header = (void *)first_skb->data;
+ // (d)wBlockLength
+ put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->tail - first_skb->data);
+ add_ndp_header(port, skb, aggr_ctx, sign);
+
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+ BUG_ON(skb->tail - first_skb->data != aggr_ctx->total_size + skb->len + padding);
+ aggr_ctx->total_size = skb->tail - first_skb->data;
+ return first_skb;
+error:
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+}
+#endif //CONFIG_USBNET_USE_SG
+#endif //CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
+
+static int mbim_unwrap_ntb(struct gether *port,
+ struct sk_buff *skb,
+ struct sk_buff_head *list)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ __le16 *tmp = (void *) skb->data;
+ unsigned index, index2;
+ unsigned dg_len, dg_len2;
+ unsigned ndp_len;
+ struct sk_buff *skb2 = NULL;
+ struct net_device *net_dev = port->ioport->net;
+ int ret = -EINVAL;
+ unsigned max_size = mbim->port.fixed_out_len;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
+ int dgram_counter;
+ u16 tci = 0;
+ u8 *c = NULL;
+
+ /* dwSignature */
+ if (get_unaligned_le32(tmp) != opts->nth_sign) {
+ pr_info("Wrong NTH SIGN, skblen %d\n",
+ skb->len);
+ print_hex_dump(KERN_INFO, "HEAD:\n", DUMP_PREFIX_ADDRESS, 32, 1,
+ skb->data, 32, false);
+
+ goto err;
+ }
+ tmp += 2;
+ /* wHeaderLength */
+ if (get_unaligned_le16(tmp++) != opts->nth_size) {
+ pr_info("Wrong NTB headersize\n");
+ goto err;
+ }
+ tmp++; /* skip wSequence */
+
+ /* (d)wBlockLength */
+ if (get_mbim(&tmp, opts->block_length) > max_size) {
+ pr_info("OUT size exceeded\n");
+ goto err;
+ }
+
+ index = get_mbim(&tmp, opts->fp_index);
+ /* MBIM 3.2 */
+ if (((index % 4) != 0) && (index < opts->nth_size)) {
+ pr_info("Bad index: %x\n",
+ index);
+ goto err;
+ }
+
+ /* walk through NDP */
+ tmp = ((void *)skb->data) + index;
+ switch (get_unaligned_le32(tmp) & cpu_to_le32(0x00ffffff)) {
+ case USB_CDC_MBIM_NDP16_IPS_SIGN:
+ case USB_CDC_MBIM_NDP32_IPS_SIGN:
+ c = (u8 *)tmp;
+ tci = c[3];
+ break;
+ case USB_CDC_MBIM_NDP16_DSS_SIGN:
+ case USB_CDC_MBIM_NDP32_DSS_SIGN:
+ c = (u8 *)tmp;
+ tci = c[3] + 256;
+ break;
+ default:
+ pr_info("unsupported NDP signature <0x%08x>\n", le32_to_cpu(tmp));
+ goto err;
+ }
+
+ tmp += 2;
+
+ ndp_len = get_unaligned_le16(tmp++);
+ /*
+ * MBIM 3.3.1
+ * entry is 2 items
+ * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
+ * minimal: struct usb_cdc_mbim_ndpX + normal entry + zero entry
+ */
+ if ((ndp_len < opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2))
+ || (ndp_len % opts->ndplen_align != 0)) {
+ pr_info("Bad NDP length: %x\n", ndp_len);
+ goto err;
+ }
+ tmp += opts->reserved1;
+ tmp += opts->next_fp_index; /* skip reserved (d)wNextFpIndex */
+ tmp += opts->reserved2;
+
+ ndp_len -= opts->ndp_size;
+ index2 = get_mbim(&tmp, opts->dgram_item_len);
+ dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
+ dgram_counter = 0;
+
+ do {
+ index = index2;
+ dg_len = dg_len2;
+ if (dg_len < 14 + crc_len) { /* ethernet header + crc */
+ pr_info("Bad dgram length: %x\n",
+ dg_len);
+ goto err;
+ }
+ if (mbim->is_crc) {
+ uint32_t crc, crc2;
+
+ crc = get_unaligned_le32(skb->data +
+ index + dg_len - crc_len);
+ crc2 = ~crc32_le(~0,
+ skb->data + index,
+ dg_len - crc_len);
+ if (crc != crc2) {
+ pr_info("Bad CRC\n");
+ goto err;
+ }
+ }
+
+ index2 = get_mbim(&tmp, opts->dgram_item_len);
+ dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
+
+ if (index2 == 0 || dg_len2 == 0) {
+ skb2 = skb;
+ } else {
+ skb2 = skb_clone(skb, GFP_ATOMIC);
+ BUG_ON(!skb2);
+ }
+
+ if (!skb_pull(skb2, index)) {
+ ret = -EOVERFLOW;
+ goto err;
+ }
+
+ skb_trim(skb2, dg_len - crc_len);
+ skb2->dev = net_dev;
+
+ MBIM_SKB_CB(skb2)->sign = MBIM_SK_CB_SIGN;
+ MBIM_SKB_CB(skb2)->tci = tci;
+
+ BUG_ON(tci > 255); //Fix this
+ /* map MBIM session to VLAN */
+ if (tci) {//maybe do it always, also for VLAN ID 0
+ pr_err("Adding VLAN\n");
+ BUG();
+ vlan_put_tag(skb2, htons(ETH_P_8021Q), tci);
+ }
+
+ skb_queue_tail(list, skb2);
+
+ ndp_len -= 2 * (opts->dgram_item_len * 2);
+
+ dgram_counter++;
+
+ if (index2 == 0 || dg_len2 == 0)
+ break;
+ } while (ndp_len > 2 * (opts->dgram_item_len * 2)); /* zero entry */
+
+ return 0;
+err:
+ skb_queue_purge(list);
+ dev_kfree_skb_any(skb);
+ return ret;
+}
+
+static struct sk_buff *mbim_process_dgram(struct gether *port,
+ struct sk_buff *skb)
+{
+ struct net_device *dev = port->ioport->net;
+ struct sk_buff *skb2 = NULL;
+ __be16 proto = htons(ETH_P_802_3);
+
+ BUG_ON(!dev || !port || !skb || !skb->data || !skb->len);
+
+
+
+ if (MBIM_SKB_CB(skb)->tci < 256) { /* IPS session? */
+
+ switch (*(skb->data) & 0xf0) {
+ case 0x40:
+ proto = htons(ETH_P_IP);
+ break;
+ case 0x60:
+#if 0
+ if (is_neigh_solicit(buf, len))
+ do_neigh_solicit(dev, buf, tci);
+#endif
+ proto = htons(ETH_P_IPV6);
+ break;
+ default:
+ goto err;
+ }
+ }
+
+ skb2 = netdev_alloc_skb_ip_align(dev, skb->len + ETH_HLEN);
+ if (!skb2)
+ goto err;
+
+ /* add an ethernet header */
+ skb_put(skb2, ETH_HLEN);
+ skb_reset_mac_header(skb2);
+ eth_hdr(skb2)->h_proto = proto;
+
+ memcpy(eth_hdr(skb2)->h_source, port->ioport->host_mac, ETH_ALEN);
+ memcpy(eth_hdr(skb2)->h_dest, dev->dev_addr, ETH_ALEN);
+
+ /* add datagram */
+ memcpy(skb_put(skb2, skb->len), skb->data, skb->len);
+#if 0
+ /* map MBIM session to VLAN */
+ if (tci)
+ vlan_put_tag(skb, htons(ETH_P_8021Q), tci);
+#endif
+err:
+ return skb2;
+}
+#endif /* CONFIG_ASR_TOE */
+
+static void mbim_disable(struct usb_function *f)
+{
+ struct f_mbim *mbim = func_to_mbim(f);
+
+ pr_info("SET DEVICE OFFLINE\n");
+#ifdef CONFIG_ASR_TOE
+ mbim->port.ueth_type = UETHER_UNKNOWN;
+#endif
+ atomic_set(&mbim->online, 0);
+ atomic_set(&mbim->response_available, 0);
+ atomic_set(&mbim->notify_count, 0);
+
+ mbim_clear_queues(mbim);
+ mbim_reset_function_queue(mbim);
+
+ if (mbim->port.in_ep->driver_data)
+ gether_disconnect(&mbim->port);
+
+ if (mbim->notify->driver_data) {
+ usb_ep_disable(mbim->notify);
+ mbim->notify->driver_data = NULL;
+ mbim->notify_desc = NULL;
+ }
+}
+
+
+static struct sk_buff *ncm_wrap_ntb(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ struct sk_buff *skb2;
+ int ncb_len, headroom;
+ int div = ntb_parameters.wNdpInDivisor;
+ int rem = ntb_parameters.wNdpInPayloadRemainder;
+ int pad = 0;
+ struct nth *nth_header;
+ struct ndp *ndp_header;
+ __le16 *tmp;
+ int ndp_align = ntb_parameters.wNdpInAlignment;
+ int ndp_pad;
+ unsigned max_size = mbim->port.fixed_in_len;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
+
+ ncb_len = opts->nth_size;
+ ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
+ ncb_len += ndp_pad;
+ ncb_len += opts->ndp_size;
+ ncb_len += 2 * 2 * opts->dgram_item_len; /* Datagram entry */
+ ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
+ pad = ALIGN(ncb_len, div) + rem - ncb_len;
+ ncb_len += pad;
+
+ if (ncb_len + skb->len + crc_len > max_size) {
+ dev_kfree_skb_any(skb);
+ return NULL;
+ }
+
+ if (unlikely(skb->cloned || skb_headroom(skb) < ncb_len
+ || skb_tailroom(skb) < crc_len)) {
+ skb2 = skb_copy_expand(skb, ncb_len,
+ crc_len,
+ GFP_ATOMIC);
+ dev_kfree_skb_any(skb);
+ if (!skb2)
+ return NULL;
+
+ skb = skb2;
+ skb2 = NULL;
+ }
+
+#ifndef CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS
+ headroom = skb_headroom(skb);
+#ifndef CONFIG_ASR_TOE
+ if (headroom >= (ncb_len + 4 * (MAX_IN_SKB_NUM + 1))) {
+ headroom = (ncb_len + 4 * MAX_IN_SKB_NUM);
+ headroom += ((u32)(skb->data - headroom) & 0x3);
+ }
+#endif
+#else
+ headroom = ncb_len;
+#endif
+ nth_header = (void *)skb_push(skb, headroom);
+ memset(nth_header, 0, headroom);
+ put_unaligned_le32(opts->nth_sign, &nth_header->dwSignature);
+ put_unaligned_le16(opts->nth_size, &nth_header->wHeaderLength);
+ put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->len); /* the first pointer is right after the NTH + align */
+ put_mbim_no_inc(NTH_FIELD(nth_header, wFpIndex), opts->fp_index, opts->nth_size + ndp_pad); /* NDP */
+ ndp_header = (void *)nth_header + opts->nth_size + ndp_pad;
+ put_unaligned_le32(opts->ndp_sign, &ndp_header->dwSignature);
+ put_unaligned_le16(ncb_len - opts->nth_size, &ndp_header->wLength);
+ if (unlikely(mbim->is_crc)) {
+ pr_err("NCM fast-path doesn't support CRC in multiple packets!!"
+ "CI needs to allocate additional space for CRC at the end of the packet.\n");
+ BUG();
+ }
+ tmp = (void *)ndp_header + opts->ndp_size; /* (d)wDatagramIndex[0] */
+ put_mbim(&tmp, opts->dgram_item_len, headroom); /* (d)wDatagramLength[0] */
+ put_mbim(&tmp, opts->dgram_item_len, skb->len - headroom); /* (d)wDatagramIndex[1] and (d)wDatagramLength[1] already zeroed */
+ return skb;
+}
+
+#ifdef CONFIG_ASR_TOE
+static struct sk_buff *ncm_wrap_ntb_multipkt(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ int ndp_align = ntb_parameters.wNdpInAlignment;
+ int ndp_pad;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ int ncb_len;
+ struct sk_buff *first_skb, *prev_skb;
+ unsigned int max_nth_size, ndp_entry_pointer_len;
+
+ ncb_len = opts->nth_size;
+ ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
+ ncb_len += ndp_pad;
+ ncb_len += opts->ndp_size;
+ ndp_entry_pointer_len = 2 * 2 * opts->dgram_item_len; /* Datagram entry */
+ ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
+
+ BUG_ON(!aggr_ctx);
+ BUG_ON(!skb);
+
+ if (skb_queue_empty(&aggr_ctx->skb_list) && list_empty(&aggr_ctx->toe_list)) {
+ struct sk_buff *old_skb = skb; /*check instead in aggregation condition that mem is shmem */
+ max_nth_size = skb_headroom(skb);
+ skb = ncm_wrap_ntb(port,skb,aggr_ctx);
+ if (likely(skb)) {
+ /*save first skb headroom for future aggregations limit*/
+ NTB_MAX_HEADER_SIZE(skb) = old_skb==skb ? max_nth_size : 0;
+ aggr_ctx->total_size = skb->len;
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+ }
+ aggr_ctx->is_toe = false;
+ return skb;
+ }
+
+ if (aggr_ctx->is_toe) {
+ aggr_ctx->pending_skb = skb;
+ first_skb = (struct sk_buff *)list_first_entry(&aggr_ctx->toe_list, struct toe_pkt_desc, list);
+ return first_skb;
+ }
+
+ first_skb = skb_peek(&aggr_ctx->skb_list);
+ prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
+
+ BUG_ON(!first_skb);
+ BUG_ON(!prev_skb);
+ BUG_ON(first_skb == prev_skb && 1 != skb_queue_len(&aggr_ctx->skb_list));
+ BUG_ON(!aggr_ctx->total_size);
+
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+}
+
+#define NCM_LONG_SKB_LEN (1000)
+#define SKB_HEADROOM_RESERVE (64)
+#define SKB_COPY_ALLOC_LENGTH (1514 + SKB_HEADROOM_RESERVE + NET_IP_ALIGN)
+
+#ifdef CONFIG_NCM_INPUT_AS_RNDIS
+static void ncm_add_rndis_header(struct sk_buff *skb)
+{
+ struct rndis_packet_msg_type *header;
+
+ if (!skb)
+ return;
+ header = (void *)skb_push(skb, sizeof(*header));
+ memset(header, 0, sizeof *header);
+ header->MessageType = cpu_to_le32(RNDIS_MSG_PACKET);
+ header->MessageLength = cpu_to_le32(skb->len);
+ header->DataOffset = cpu_to_le32(36);
+ header->DataLength = cpu_to_le32(skb->len - sizeof(*header));
+ v7_dma_flush_range((void *)header, (void *)((u32)header + sizeof(*header)));
+}
+#endif
+
+/* only short skb is copy by SW or DMA */
+static struct sk_buff *ncm_copy_skb(struct sk_buff *skb, unsigned offset, unsigned len)
+{
+ struct sk_buff *skb2;
+
+ skb2 = bm_usb_alloc_skb(SKB_COPY_ALLOC_LENGTH, GFP_ATOMIC);
+ if (!skb2)
+ return NULL;
+
+ skb_reserve(skb2, SKB_HEADROOM_RESERVE);
+ memcpy(skb_put(skb2, len), (skb->data + offset), len);
+
+ return skb2;
+}
+
+static int ncm_unwrap_ntb(struct gether *port,
+ struct sk_buff *skb,
+ struct sk_buff_head *list)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ __le16 *tmp = (void *) skb->data;
+ unsigned index, index2;
+ unsigned dg_len, dg_len2;
+ unsigned ndp_len;
+ struct sk_buff *skb2;
+ int ret = -EINVAL;
+ unsigned max_size = le32_to_cpu(ntb_parameters.dwNtbOutMaxSize);
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
+ int dgram_counter, nocopy_dgram_counter;
+ bool skb_meet_nocopy = false;
+ unsigned nocopy_offset, nocopy_len;
+
+
+ /* dwSignature */
+ if (get_unaligned_le32(tmp) != opts->nth_sign) {
+ pr_err("Wrong NTH SIGN, skblen %d\n",
+ skb->len);
+ print_hex_dump(KERN_INFO, "HEAD:\n", DUMP_PREFIX_ADDRESS, 32, 1,
+ skb->data, 32, false);
+
+ goto err;
+ }
+ tmp += 2;
+ /* wHeaderLength */
+ if (get_unaligned_le16(tmp++) != opts->nth_size) {
+ pr_err("Wrong NTB headersize\n");
+ goto err;
+ }
+ tmp++; /* skip wSequence */
+
+ /* (d)wBlockLength */
+ if (get_mbim(&tmp, opts->block_length) > max_size) {
+ pr_err("OUT size exceeded\n");
+ goto err;
+ }
+
+ index = get_mbim(&tmp, opts->fp_index);
+ /* NCM 3.2 */
+ if (((index % 4) != 0) && (index < opts->nth_size)) {
+ pr_err("Bad index: %x\n",
+ index);
+ goto err;
+ }
+
+ /* walk through NDP */
+ tmp = ((void *)skb->data) + index;
+ if (get_unaligned_le32(tmp) != opts->ndp_sign) {
+ pr_err("Wrong NDP SIGN\n");
+ goto err;
+ }
+ tmp += 2;
+
+ ndp_len = get_unaligned_le16(tmp++);
+ /*
+ * NCM 3.3.1
+ * entry is 2 items
+ * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
+ * minimal: struct usb_cdc_ncm_ndpX + normal entry + zero entry
+ */
+ if ((ndp_len < opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2))
+ || (ndp_len % opts->ndplen_align != 0)) {
+ pr_err("Bad NDP length: %x\n", ndp_len);
+ goto err;
+ }
+ tmp += opts->reserved1;
+ tmp += opts->next_fp_index; /* skip reserved (d)wNextFpIndex */
+ tmp += opts->reserved2;
+
+ ndp_len -= opts->ndp_size;
+ index2 = get_mbim(&tmp, opts->dgram_item_len);
+ dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
+ dgram_counter = 0;
+
+ do {
+ index = index2;
+ dg_len = dg_len2;
+
+ if (!skb_meet_nocopy && dg_len > NCM_LONG_SKB_LEN) {
+ skb_meet_nocopy = true;
+ nocopy_offset = index;
+ nocopy_len = dg_len;
+ nocopy_dgram_counter = dgram_counter;
+ }
+
+ if (dg_len < 14 + crc_len) { /* ethernet header + crc */
+ pr_err("Bad dgram length: %x\n",
+ dg_len);
+ goto err;
+ }
+ if (mbim->is_crc) {
+ uint32_t crc, crc2;
+
+ crc = get_unaligned_le32(skb->data +
+ index + dg_len - crc_len);
+ crc2 = ~crc32_le(~0,
+ skb->data + index,
+ dg_len - crc_len);
+ if (crc != crc2) {
+ pr_err("Bad CRC\n");
+ goto err;
+ }
+ }
+
+ index2 = get_mbim(&tmp, opts->dgram_item_len);
+ dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
+
+ /* last skb */
+ if (index2 == 0 || dg_len2 == 0) {
+ /* only one skb, no copy */
+ if (dgram_counter == 0) {
+ skb2 = skb;
+ goto skb_nocpy_pull;
+ } else {
+ if (skb_meet_nocopy) {
+ /* last skb is no cpy */
+ if (nocopy_dgram_counter == (dgram_counter - 1)) {
+ skb2 = skb;
+ goto skb_nocpy_pull;
+ } else {
+ /* last pkt need to be copied
+ * handle the last skb and non-copy skb
+ */
+ skb2 = ncm_copy_skb(skb, index, dg_len);
+ if (skb2 == NULL) {
+ pr_info("%s: skb cp err\n", __func__);
+ goto err;
+ } else {
+ skb_trim(skb2, dg_len - crc_len);
+ skb_queue_tail(list, skb2);
+
+ /* handle the non copy skb */
+ skb2 = skb;
+ index = nocopy_offset;
+ dg_len = nocopy_len;
+ goto skb_nocpy_pull;
+ }
+ }
+ } else {
+ skb2 = skb;
+ goto skb_nocpy_pull;
+ }
+ }
+ } else {
+ if (skb_meet_nocopy && nocopy_dgram_counter == dgram_counter) {
+ /* TODO: skip the skb copy, do nothing? */
+ goto skip_skb_copy;
+ } else {
+ skb2 = ncm_copy_skb(skb, index, dg_len);
+ if (skb2 == NULL) {
+ pr_info("%s: skb cp err2\n", __func__);
+ goto err;
+ } else {
+ goto skb_nopull;
+ }
+ }
+ }
+
+skb_nocpy_pull:
+ if (!skb_pull(skb2, index)) {
+ ret = -EOVERFLOW;
+ pr_err("%s: skb2 pull error\n", __func__);
+ goto err;
+ }
+
+skb_nopull:
+ skb_trim(skb2, dg_len - crc_len);
+#ifdef CONFIG_NCM_INPUT_AS_RNDIS
+ ncm_add_rndis_header(skb2);
+#endif
+ skb_queue_tail(list, skb2);
+
+skip_skb_copy:
+ ndp_len -= 2 * (opts->dgram_item_len * 2);
+
+ dgram_counter++;
+
+ if (index2 == 0 || dg_len2 == 0)
+ break;
+ } while (ndp_len > 2 * (opts->dgram_item_len * 2)); /* zero entry */
+
+ VDBG(port->func.config->cdev,
+ "Parsed NTB with %d frames\n", dgram_counter);
+ return 0;
+err:
+ skb_queue_purge(list);
+ dev_kfree_skb_any(skb);
+ return ret;
+}
+
+#else
+#ifdef CONFIG_USB_G_NCM_MULT_PKT_SUPPORT
+#ifdef CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS
+static struct sk_buff *ncm_wrap_ntb_multipkt(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ int padding, aggregate_length, skb_qlen;
+ struct nth *nth_header;
+ int ndp_align = ntb_parameters.wNdpInAlignment;
+ int ndp_pad;
+ unsigned max_size = mbim->port.fixed_in_len;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ int ncb_len;
+ int single_entry_ndp_size = opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2);
+ struct sk_buff *first_skb, *prev_skb;
+
+ BUG_ON(!aggr_ctx);
+ BUG_ON(!skb);
+
+ skb_qlen = skb_queue_len(&aggr_ctx->skb_list);
+ ncb_len = opts->nth_size;
+ ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
+ ncb_len += ndp_pad;
+ ncb_len += opts->ndp_size;
+ ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
+
+ if (skb_queue_empty(&aggr_ctx->skb_list)) {
+ skb = ncm_wrap_ntb(port,skb,aggr_ctx);
+ if (likely(skb)) {
+ aggr_ctx->total_size = skb->len;
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+ }
+ return skb;
+ }
+
+ first_skb = skb_peek(&aggr_ctx->skb_list);
+ prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
+
+ BUG_ON(!first_skb);
+ BUG_ON(!prev_skb);
+ BUG_ON(first_skb == prev_skb && 1!=skb_queue_len(&aggr_ctx->skb_list));
+ BUG_ON(!aggr_ctx->total_size);
+ padding = (unsigned)skb->data - single_entry_ndp_size - (unsigned)prev_skb->tail;
+ aggregate_length = aggr_ctx->total_size + skb->len + padding + single_entry_ndp_size;
+ BUG_ON(aggregate_length != skb->tail - first_skb->data);
+
+ /* ncm aggregation condition */
+ if (aggregate_length > max_size ||
+ (mbim->ntb_max_datagrams != 0 && mbim->ntb_max_datagrams <= skb_qlen) ||
+ skb_qlen >= MAX_DGRAMS_AGGREGATION ||
+ skb_cloned(skb) ||
+ skb_headroom(skb) < single_entry_ndp_size ||
+ padding > AGGR_MAX_PADDING ||
+ padding < 0) {
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+ }
+
+ /* update NTH */
+ nth_header = (void *)first_skb->data;
+ /* (d)wBlockLength */
+ put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->tail - first_skb->data);
+
+ add_ndp_header(port, skb, aggr_ctx, opts->ndp_sign);
+
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+ BUG_ON(skb->tail - first_skb->data != aggr_ctx->total_size + skb->len + padding);
+ aggr_ctx->total_size = skb->tail - first_skb->data;
+ /* aggr_ctx->total_padding += padding;*/
+ return first_skb;
+}
+#else
+static struct sk_buff *ncm_wrap_ntb_multipkt(struct gether *port,
+ struct sk_buff *skb, struct aggr_ctx *aggr_ctx)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ int padding, aggregate_length, skb_qlen;
+ __le16 *tmp;
+ int ndp_align = ntb_parameters.wNdpInAlignment;
+ int ndp_pad;
+ struct nth *nth_header;
+ struct ndp *ndp_header;
+ unsigned max_size = mbim->port.fixed_in_len;
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ int ncb_len;
+ struct sk_buff *first_skb, *prev_skb;
+ unsigned int max_nth_size, ndp_entry_pointer_len;
+
+ ncb_len = opts->nth_size;
+ ndp_pad = ALIGN(ncb_len, ndp_align) - ncb_len;
+ ncb_len += ndp_pad;
+ ncb_len += opts->ndp_size;
+ ndp_entry_pointer_len = 2 * 2 * opts->dgram_item_len; /* Datagram entry */
+ ncb_len += 2 * 2 * opts->dgram_item_len; /* Zero datagram entry */
+
+ BUG_ON(!aggr_ctx);
+ BUG_ON(!skb);
+
+ if (skb_queue_empty(&aggr_ctx->skb_list)) {
+ struct sk_buff *old_skb = skb; /*check instead in aggregation condition that mem is shmem */
+ max_nth_size = skb_headroom(skb);
+ skb = ncm_wrap_ntb(port,skb,aggr_ctx);
+ if (likely(skb)) {
+ /*save first skb headroom for future aggregations limit*/
+ if (max_nth_size > (ncb_len + 4 * MAX_IN_SKB_NUM)) {
+ max_nth_size = (ncb_len + 4 * MAX_IN_SKB_NUM);
+ }
+ NTB_MAX_HEADER_SIZE(skb) = (old_skb==skb)? max_nth_size : 0;
+ aggr_ctx->total_size = skb->len;
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+ }
+ return skb;
+ }
+
+ first_skb = skb_peek(&aggr_ctx->skb_list);
+ prev_skb = skb_peek_tail(&aggr_ctx->skb_list);
+ max_nth_size = NTB_MAX_HEADER_SIZE(first_skb);
+
+ BUG_ON(!first_skb);
+ BUG_ON(!prev_skb);
+ BUG_ON(first_skb == prev_skb && 1!=skb_queue_len(&aggr_ctx->skb_list));
+ BUG_ON(!aggr_ctx->total_size);
+
+ padding = (unsigned)skb->data - (unsigned)prev_skb->tail;
+ aggregate_length = aggr_ctx->total_size + skb->len + padding;
+ skb_qlen = skb_queue_len(&aggr_ctx->skb_list);
+
+ /* ncm aggregation condition */
+ if (aggregate_length >= max_size ||
+ (max_nth_size < (ncb_len + ndp_entry_pointer_len * (skb_qlen + 1))) ||
+ (mbim->ntb_max_datagrams != 0 && mbim->ntb_max_datagrams <= skb_qlen) ||
+ skb_qlen >= MAX_DGRAMS_AGGREGATION ||
+ skb_cloned(skb) ||
+ padding > AGGR_MAX_PADDING ||
+ padding < 0) {
+ aggr_ctx->pending_skb = skb;
+ return first_skb;
+ }
+
+ /* update NTH */
+ nth_header = (void *)first_skb->data;
+ /* (d)wBlockLength */
+ put_mbim_no_inc(NTH_FIELD(nth_header, wBlockLength), opts->block_length, skb->tail - first_skb->data);
+
+ /* update NDP */
+ ndp_header = (void *)nth_header + opts->nth_size + ndp_pad;
+ /* wLength */
+ put_unaligned_le16(opts->ndp_size + ndp_entry_pointer_len * (skb_qlen + 2), &ndp_header->wLength);
+
+ /* skip previous packets ndp entries*/
+ tmp = (void *)ndp_header + opts->ndp_size + ndp_entry_pointer_len * skb_qlen;
+ /* (d)wDatagramIndex[i] */
+ put_mbim(&tmp, opts->dgram_item_len, skb->data - first_skb->data);
+ /* (d)wDatagramLength[i] */
+ put_mbim(&tmp, opts->dgram_item_len, skb->len);
+ /* (d)wDatagramIndex[N] and (d)wDatagramLength[N] already zeroed */
+
+ skb_queue_tail(&aggr_ctx->skb_list, skb);
+ BUG_ON(skb->tail - first_skb->data != aggr_ctx->total_size + skb->len + padding);
+ aggr_ctx->total_size = skb->tail - first_skb->data;
+ //aggr_ctx->total_padding += padding;
+ return first_skb;
+}
+#endif
+#endif
+
+static int ncm_unwrap_ntb(struct gether *port,
+ struct sk_buff *skb,
+ struct sk_buff_head *list)
+{
+ struct f_mbim *mbim = func_to_mbim(&port->func);
+ __le16 *tmp = (void *) skb->data;
+ unsigned index, index2;
+ unsigned dg_len, dg_len2;
+ unsigned ndp_len;
+ struct sk_buff *skb2;
+ int ret = -EINVAL;
+ unsigned max_size = le32_to_cpu(ntb_parameters.dwNtbOutMaxSize);
+ struct ndp_parser_opts *opts = mbim->parser_opts;
+ unsigned crc_len = mbim->is_crc ? sizeof(uint32_t) : 0;
+ int dgram_counter;
+
+ /* dwSignature */
+ if (get_unaligned_le32(tmp) != opts->nth_sign) {
+ pr_err("Wrong NTH SIGN, skblen %d\n",
+ skb->len);
+ print_hex_dump(KERN_INFO, "HEAD:\n", DUMP_PREFIX_ADDRESS, 32, 1,
+ skb->data, 32, false);
+
+ goto err;
+ }
+ tmp += 2;
+ /* wHeaderLength */
+ if (get_unaligned_le16(tmp++) != opts->nth_size) {
+ pr_err("Wrong NTB headersize\n");
+ goto err;
+ }
+ tmp++; /* skip wSequence */
+
+ /* (d)wBlockLength */
+ if (get_mbim(&tmp, opts->block_length) > max_size) {
+ pr_err("OUT size exceeded\n");
+ goto err;
+ }
+
+ index = get_mbim(&tmp, opts->fp_index);
+ /* NCM 3.2 */
+ if (((index % 4) != 0) && (index < opts->nth_size)) {
+ pr_err("Bad index: %x\n",
+ index);
+ goto err;
+ }
+
+ /* walk through NDP */
+ tmp = ((void *)skb->data) + index;
+ if (get_unaligned_le32(tmp) != opts->ndp_sign) {
+ pr_err("Wrong NDP SIGN\n");
+ goto err;
+ }
+ tmp += 2;
+
+ ndp_len = get_unaligned_le16(tmp++);
+ /*
+ * NCM 3.3.1
+ * entry is 2 items
+ * item size is 16/32 bits, opts->dgram_item_len * 2 bytes
+ * minimal: struct usb_cdc_ncm_ndpX + normal entry + zero entry
+ */
+ if ((ndp_len < opts->ndp_size + 2 * 2 * (opts->dgram_item_len * 2))
+ || (ndp_len % opts->ndplen_align != 0)) {
+ pr_err("Bad NDP length: %x\n", ndp_len);
+ goto err;
+ }
+ tmp += opts->reserved1;
+ tmp += opts->next_fp_index; /* skip reserved (d)wNextFpIndex */
+ tmp += opts->reserved2;
+
+ ndp_len -= opts->ndp_size;
+ index2 = get_mbim(&tmp, opts->dgram_item_len);
+ dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
+ dgram_counter = 0;
+
+ do {
+ index = index2;
+ dg_len = dg_len2;
+ if (dg_len < 14 + crc_len) { /* ethernet header + crc */
+ pr_err("Bad dgram length: %x\n",
+ dg_len);
+ goto err;
+ }
+ if (mbim->is_crc) {
+ uint32_t crc, crc2;
+
+ crc = get_unaligned_le32(skb->data +
+ index + dg_len - crc_len);
+ crc2 = ~crc32_le(~0,
+ skb->data + index,
+ dg_len - crc_len);
+ if (crc != crc2) {
+ pr_err("Bad CRC\n");
+ goto err;
+ }
+ }
+
+ index2 = get_mbim(&tmp, opts->dgram_item_len);
+ dg_len2 = get_mbim(&tmp, opts->dgram_item_len);
+
+ if (index2 == 0 || dg_len2 == 0) {
+ skb2 = skb;
+ } else {
+ skb2 = skb_clone(skb, GFP_ATOMIC);
+ if (skb2 == NULL)
+ goto err;
+ }
+
+ if (!skb_pull(skb2, index)) {
+ ret = -EOVERFLOW;
+ goto err;
+ }
+
+ skb_trim(skb2, dg_len - crc_len);
+ skb_queue_tail(list, skb2);
+
+ ndp_len -= 2 * (opts->dgram_item_len * 2);
+
+ dgram_counter++;
+
+ if (index2 == 0 || dg_len2 == 0)
+ break;
+ } while (ndp_len > 2 * (opts->dgram_item_len * 2)); /* zero entry */
+
+ VDBG(port->func.config->cdev,
+ "Parsed NTB with %d frames\n", dgram_counter);
+ return 0;
+err:
+ skb_queue_purge(list);
+ dev_kfree_skb_any(skb);
+ return ret;
+}
+#endif
+
+
+
+
+/*-------------------------------------------------------------------------*/
+
+/*
+ * Callbacks let us notify the host about connect/disconnect when the
+ * net device is opened or closed.
+ *
+ * For testing, note that link states on this side include both opened
+ * and closed variants of:
+ *
+ * - disconnected/unconfigured
+ * - configured but inactive (data alt 0)
+ * - configured and active (data alt 1)
+ *
+ * Each needs to be tested with unplug, rmmod, SET_CONFIGURATION, and
+ * SET_INTERFACE (altsetting). Remember also that "configured" doesn't
+ * imply the host is actually polling the notification endpoint, and
+ * likewise that "active" doesn't imply it's actually using the data
+ * endpoints for traffic.
+ */
+
+static void __maybe_unused mbim_net_open(struct gether *geth)
+{
+ struct f_mbim *mbim = func_to_mbim(&geth->func);
+
+ spin_lock(&mbim->lock);
+ mbim->is_open = true;
+ mbim_notify(mbim);
+ spin_unlock(&mbim->lock);
+}
+
+static void __maybe_unused mbim_net_close(struct gether *geth)
+{
+ struct f_mbim *mbim = func_to_mbim(&geth->func);
+
+ spin_lock(&mbim->lock);
+ mbim->is_open = false;
+ mbim_notify(mbim);
+ spin_unlock(&mbim->lock);
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* ethernet function driver setup/binding */
+
+static int
+mbim_bind(struct usb_configuration *c, struct usb_function *f)
+{
+ struct usb_composite_dev *cdev = c->cdev;
+ struct f_mbim *mbim = func_to_mbim(f);
+ int status;
+ struct usb_ep *ep;
+
+ /* allocate instance-specific interface IDs */
+ status = usb_interface_id(c, f);
+ if (status < 0)
+ goto fail;
+ mbim->ctrl_id = status;
+ mbim_iad_desc.bFirstInterface = status;
+
+ ncm_control_intf.bInterfaceNumber = status;
+ mbim_control_intf.bInterfaceNumber = status;
+ ncm_mbim_control_intf.bInterfaceNumber = status;
+ mbim_union_desc.bMasterInterface0 = status;
+
+ status = usb_interface_id(c, f);
+ if (status < 0)
+ goto fail;
+ mbim->data_id = status;
+
+ ncm_data_nop_intf.bInterfaceNumber = status;
+ ncm_data_intf.bInterfaceNumber = status;
+ mbim_data_nop_intf.bInterfaceNumber = status;
+ mbim_data_intf.bInterfaceNumber = status;
+ mbim_union_desc.bSlaveInterface0 = status;
+ ncm_mbim_data_intf.bInterfaceNumber = status;
+
+ status = -ENODEV;
+
+ /* allocate instance-specific endpoints */
+ ep = usb_ep_autoconfig(cdev->gadget, &fs_mbim_in_desc);
+ if (!ep)
+ goto fail;
+ mbim->port.in_ep = ep;
+ ep->driver_data = cdev; /* claim */
+
+ ep = usb_ep_autoconfig(cdev->gadget, &fs_mbim_out_desc);
+ if (!ep)
+ goto fail;
+ mbim->port.out_ep = ep;
+ ep->driver_data = cdev; /* claim */
+
+ ep = usb_ep_autoconfig(cdev->gadget, &fs_mbim_notify_desc);
+ if (!ep)
+ goto fail;
+ mbim->notify = ep;
+ ep->driver_data = cdev; /* claim */
+
+ status = -ENOMEM;
+
+ /* allocate notification request and buffer */
+ mbim->notify_req = mbim_alloc_req(ep, MBIM_STATUS_BYTECOUNT);
+ if (!mbim->notify_req || !mbim->notify_req->buf)
+ goto fail;
+
+ mbim->notify_req->context = mbim;
+ mbim->notify_req->complete = mbim_notify_complete;
+
+ g_notify_req = mbim->notify_req;
+ /*
+ * support all relevant hardware speeds... we expect that when
+ * hardware is dual speed, all bulk-capable endpoints work at
+ * both speeds
+ */
+ hs_mbim_in_desc.bEndpointAddress =
+ fs_mbim_in_desc.bEndpointAddress;
+ hs_mbim_out_desc.bEndpointAddress =
+ fs_mbim_out_desc.bEndpointAddress;
+ hs_mbim_notify_desc.bEndpointAddress =
+ fs_mbim_notify_desc.bEndpointAddress;
+
+ ss_mbim_in_desc.bEndpointAddress =
+ fs_mbim_in_desc.bEndpointAddress;
+ ss_mbim_out_desc.bEndpointAddress =
+ fs_mbim_out_desc.bEndpointAddress;
+ ss_mbim_notify_desc.bEndpointAddress =
+ fs_mbim_notify_desc.bEndpointAddress;
+
+ /* copy descriptors, and track endpoint copies */
+ switch (mbim->mode) {
+ case MODE_NCM:
+ status = usb_assign_descriptors(f, ncm_fs_function, ncm_hs_function,
+ ncm_ss_function, ncm_ss_function);
+ break;
+ case MODE_MBIM:
+ status = usb_assign_descriptors(f, mbim_fs_function, mbim_hs_function,
+ mbim_ss_function, mbim_ss_function);
+ break;
+ case MODE_NCM_MBIM:
+ status = usb_assign_descriptors(f, ncm_mbim_fs_function, ncm_mbim_hs_function,
+ ncm_mbim_ss_function, ncm_mbim_ss_function);
+ break;
+ default:
+ BUG();
+ }
+#if 0
+ /*
+ * support all relevant hardware speeds... we expect that when
+ * hardware is dual speed, all bulk-capable endpoints work at
+ * both speeds
+ */
+ if (gadget_is_dualspeed(c->cdev->gadget)) {
+ hs_mbim_in_desc.bEndpointAddress =
+ fs_mbim_in_desc.bEndpointAddress;
+ hs_mbim_out_desc.bEndpointAddress =
+ fs_mbim_out_desc.bEndpointAddress;
+ hs_mbim_notify_desc.bEndpointAddress =
+ fs_mbim_notify_desc.bEndpointAddress;
+
+ /* copy descriptors, and track endpoint copies */
+ switch (mbim->mode) {
+ case MODE_NCM:
+ f->hs_descriptors = usb_copy_descriptors(ncm_hs_function);
+ break;
+ case MODE_MBIM:
+ f->hs_descriptors = usb_copy_descriptors(mbim_hs_function);
+ break;
+ case MODE_NCM_MBIM:
+ f->hs_descriptors = usb_copy_descriptors(ncm_mbim_hs_function);
+ break;
+ default:
+ BUG();
+ }
+ if (!f->hs_descriptors)
+ goto fail;
+ }
+#endif
+ /*
+ * NOTE: all that is done without knowing or caring about
+ * the network link ... which is unavailable to this code
+ * until we're activated via set_alt().
+ */
+
+ if (mbim->mode == MODE_NCM) {
+ mbim->port.open = mbim_net_open;
+ mbim->port.close = mbim_net_close;
+ } else {
+ /* Ruslan: originally was commented out in this driver*/
+ mbim->port.open = NULL;
+ mbim->port.close = NULL;
+ }
+
+ pr_info("%s speed IN/%s OUT/%s NOTIFY/%s\n",
+ gadget_is_superspeed(c->cdev->gadget) ? "super" :
+ gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
+ mbim->port.in_ep->name, mbim->port.out_ep->name,
+ mbim->notify->name);
+#ifdef CONFIG_ASR_TOE
+ if (mbim->mode == MODE_NCM) {
+ mbim->port.ueth_type = UETHER_NCM;
+ } else {
+ mbim->port.ueth_type = UETHER_MBIM;
+ }
+#endif
+
+ return 0;
+
+fail:
+ pr_err("%s failed to bind, err %d\n", f->name, status);
+
+ usb_free_all_descriptors(f);
+
+ if (mbim->notify_req) {
+ kfree(mbim->notify_req->buf);
+ usb_ep_free_request(mbim->notify, mbim->notify_req);
+ }
+
+ /* we might as well release our claims on endpoints */
+ if (mbim->notify)
+ mbim->notify->driver_data = NULL;
+ if (mbim->port.out_ep)
+ mbim->port.out_ep->driver_data = NULL;
+ if (mbim->port.in_ep)
+ mbim->port.in_ep->driver_data = NULL;
+
+ return status;
+}
+
+static void
+mbim_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+ struct f_mbim *mbim = func_to_mbim(f);
+
+#ifdef CONFIG_ASR_TOE
+ mbim->port.ueth_type = UETHER_UNKNOWN;
+#endif
+ usb_free_all_descriptors(f);
+
+ kfree(mbim->notify_req->buf);
+ g_notify_req = NULL;
+ usb_ep_free_request(mbim->notify, mbim->notify_req);
+ #if defined(CONFIG_CPU_ASR18XX) && defined(CONFIG_USB_MVC2)
+ if (gadget_current_is_dualspeed(c->cdev->gadget))
+ mbim_string_defs[0].id = 0;
+ #endif
+}
+
+/**
+ * mbim_bind_config - add MBIM link to a configuration
+ * @c: the configuration to support the network link
+ * @ethaddr: a buffer in which the ethernet address of the host side
+ * side of the link was recorded
+ * Context: single threaded during gadget setup
+ *
+ * Returns zero on success, else negative errno.
+ *
+ * Caller must have called @gether_setup(). Caller is also responsible
+ * for calling @gether_cleanup() before module unload.
+ */
+int mbim_bind_config(struct usb_configuration *c, u8 ethaddr[ETH_ALEN], struct eth_dev *dev, enum ncm_mbim_mode mode)
+{
+ struct f_mbim *mbim;
+ int status, i;
+
+#ifdef CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS
+ pr_info("CONFIG_USB_G_NCM_NON_SEQUENTIAL_NDPS defined\n");
+#endif
+ if (!can_support_ecm(c->cdev->gadget) || !ethaddr)
+ return -EINVAL;
+
+ /* maybe allocate device-global string IDs */
+ if (mbim_string_defs[0].id == 0) {
+
+ /* control interface label */
+ status = usb_string_id(c->cdev);
+ if (status < 0)
+ return status;
+ mbim_string_defs[STRING_CTRL_IDX].id = status;
+ mbim_control_intf.iInterface = status;
+
+ /* data interface label */
+ status = usb_string_id(c->cdev);
+ if (status < 0)
+ return status;
+ mbim_string_defs[STRING_DATA_IDX].id = status;
+ mbim_data_nop_intf.iInterface = status;
+ mbim_data_intf.iInterface = status;
+
+ /* MAC address */
+ status = usb_string_id(c->cdev);
+ if (status < 0)
+ return status;
+ mbim_string_defs[STRING_MAC_IDX].id = status;
+ ether_desc.iMACAddress = status;
+
+ /* IAD */
+ status = usb_string_id(c->cdev);
+ if (status < 0)
+ return status;
+ mbim_string_defs[STRING_IAD_IDX].id = status;
+ mbim_iad_desc.iFunction = status;
+ }
+
+ /* allocate and initialize one new instance */
+ mbim = mbim_ports[0].port;
+ if (!mbim) {
+ pr_info("mbim struct not allocated\n");
+ return -ENOMEM;
+ }
+
+ /* export host's Ethernet address in CDC format */
+ snprintf(mbim->dev_addr, sizeof mbim->dev_addr,
+ "%02X%02X%02X%02X%02X%02X\n",
+ ethaddr[0], ethaddr[1], ethaddr[2],
+ ethaddr[3], ethaddr[4], ethaddr[5]);
+ mbim_string_defs[1].s = mbim->dev_addr;
+
+ mbim->host_addr[0] = 0x00;
+ mbim->host_addr[1] = 0xFF;
+ mbim->host_addr[2] = 0xFF;
+ mbim->host_addr[3] = 0xFF;
+ mbim->host_addr[4] = 0xFF;
+ mbim->host_addr[5] = 0x00;
+
+ spin_lock_init(&mbim->lock);
+ mbim_reset_values(mbim);
+ mbim->port.is_fixed = true;
+ mbim->port.ioport = dev;
+ mbim->mode = mode;
+
+ switch (mbim->mode) {
+ case MODE_NCM:
+ mbim->port.func.name = "ncm";
+ break;
+ case MODE_MBIM:
+ mbim->port.func.name = "mbim";
+ break;
+ case MODE_NCM_MBIM:
+ mbim->port.func.name = "ncm_mbim";
+ break;
+ default:
+ BUG();
+ }
+
+ mbim->port.func.strings = mbim_strings;
+ /* descriptors are per-instance copies */
+ mbim->port.func.bind = mbim_bind;
+ mbim->port.func.unbind = mbim_unbind;
+ mbim->port.func.set_alt = mbim_set_alt;
+ mbim->port.func.get_alt = mbim_get_alt;
+ mbim->port.func.setup = mbim_setup;
+ mbim->port.func.disable = mbim_disable;
+
+ INIT_LIST_HEAD(&mbim->cpkt_req_q);
+ INIT_LIST_HEAD(&mbim->cpkt_resp_q);
+
+ if (mbim->mode == MODE_NCM || mbim->mode == MODE_NCM_MBIM) {
+#ifdef CONFIG_USB_G_NCM_MULT_PKT_SUPPORT
+ mbim->port.wrap = ncm_wrap_ntb_multipkt;
+#else
+ mbim->port.wrap = ncm_wrap_ntb;
+#endif
+ mbim->port.unwrap = ncm_unwrap_ntb;
+ mbim->port.unwrap_fixup = NULL;
+ }
+ if (mbim->mode == MODE_MBIM) {
+#ifdef CONFIG_USB_G_MBIM_MULT_PKT_SUPPORT
+ mbim->port.wrap = mbim_wrap_ntb_multipkt;
+#ifdef CONFIG_USBNET_USE_SG
+ mbim->port.is_sg_mode = 1;
+#endif
+#else
+ mbim->port.wrap = mbim_wrap_ntb;
+#endif
+ mbim->port.unwrap = mbim_unwrap_ntb;
+#ifndef CONFIG_ASR_TOE
+ mbim->port.unwrap_fixup = mbim_process_dgram;
+#endif
+ }
+
+ for (i = (ETH_ALEN-2); i < (ETH_ALEN-1); i++) {
+ mbim->host_addr[i] = i;
+ }
+
+ status = usb_add_function(c, &mbim->port.func);
+ if (status) {
+ mbim_string_defs[1].s = NULL;
+ kfree(mbim);
+ }
+ return status;
+}
+
+/* ------------ MBIM DRIVER File Operations API for USER SPACE ------------ */
+
+#define MBIM_BULK_BUFFER_SIZE 4096
+#define MBIM_IOCTL_MAGIC 'o'
+#define MBIM_GET_NTB_SIZE _IOR(MBIM_IOCTL_MAGIC, 2, u32)
+#define MBIM_GET_DATAGRAM_COUNT _IOR(MBIM_IOCTL_MAGIC, 3, u16)
+
+/* temporary variable used between mbim_open() and mbim_gadget_bind() */
+static struct f_mbim *_mbim_dev;
+static unsigned int nr_mbim_ports;
+static int mbim_inited = 0;
+static ssize_t
+mbim_read(struct file *fp, char __user *buf, size_t count, loff_t *pos)
+{
+ struct f_mbim *dev = fp->private_data;
+ struct ctrl_pkt *cpkt = NULL;
+ int ret = 0;
+
+ pr_debug("(%d)\n", count);
+
+ if (!dev) {
+ pr_err("Received NULL mbim pointer\n");
+ return -ENODEV;
+ }
+
+ if (dev->mode != MODE_MBIM && dev->mode != MODE_NCM_MBIM) {
+ pr_err("NOT mbim mode\n");
+ return -ENODEV;
+ }
+
+ if (count > MBIM_BULK_BUFFER_SIZE) {
+ pr_err("Buffer size is too big %d, should be at most %d\n",
+ count, MBIM_BULK_BUFFER_SIZE);
+ return -EINVAL;
+ }
+
+ if (mbim_lock(&dev->read_excl)) {
+ pr_err("Previous reading is not finished yet\n");
+ return -EBUSY;
+ }
+
+ if (atomic_read(&dev->error)) {
+ mbim_unlock(&dev->read_excl);
+ return -EIO;
+ }
+
+ while (list_empty(&dev->cpkt_req_q)) {
+ pr_debug("Requests list is empty. Wait.\n");
+ ret = wait_event_interruptible(dev->read_wq,
+ !list_empty(&dev->cpkt_req_q));
+ if (ret < 0) {
+ pr_debug("Waiting interrupted\n");
+ mbim_unlock(&dev->read_excl);
+ return 0;
+ }
+ pr_debug("Received request packet\n");
+ }
+
+ cpkt = list_first_entry(&dev->cpkt_req_q, struct ctrl_pkt,
+ list);
+ if (cpkt->len > count) {
+ mbim_unlock(&dev->read_excl);
+ pr_err("cpkt size too big:%d > buf size:%d\n",
+ cpkt->len, count);
+ return -ENOMEM;
+ }
+
+ pr_debug("cpkt size:%d trans_id:%d\n", cpkt->len, cpkt->transaction_id);
+
+ list_del(&cpkt->list);
+ mbim_unlock(&dev->read_excl);
+
+ ret = copy_to_user(buf, cpkt->buf, cpkt->len);
+ if (ret) {
+ pr_err("copy_to_user failed: err %d\n", ret);
+ ret = 0;
+ } else {
+ pr_debug("copied %d bytes to user\n", cpkt->len);
+ ret = cpkt->len;
+ }
+
+ mbim_free_ctrl_pkt(cpkt);
+
+ return ret;
+}
+
+static ssize_t
+mbim_write(struct file *fp, const char __user *buf, size_t count, loff_t *pos)
+{
+ struct f_mbim *dev = fp->private_data;
+ struct ctrl_pkt *cpkt = NULL;
+ int ret = 0;
+ struct usb_cdc_mbim_ctrl_msg *ctrl_msg = (struct usb_cdc_mbim_ctrl_msg *)buf;
+
+ pr_debug("(%d)\n", count);
+
+ if (!dev) {
+ pr_err("Received NULL mbim pointer\n");
+ return -ENODEV;
+ }
+
+ if (dev->mode != MODE_MBIM && dev->mode != MODE_NCM_MBIM) {
+ pr_err("NOT mbim mode\n");
+ return -ENODEV;
+ }
+
+ if (!count) {
+ pr_err("zero length ctrl pkt\n");
+ return -ENODEV;
+ }
+
+ if (count > MAX_CTRL_PKT_SIZE) {
+ pr_err("given pkt size too big:%d > max_pkt_size:%d\n",
+ count, MAX_CTRL_PKT_SIZE);
+ return -ENOMEM;
+ }
+
+ if (mbim_lock(&dev->write_excl)) {
+ pr_err("Previous writing not finished yet\n");
+ return -EBUSY;
+ }
+
+ if (!atomic_read(&dev->online)) {
+ pr_info("MBIM not connected\n");
+ mbim_unlock(&dev->write_excl);
+ return -EPIPE;
+ }
+
+ /* Hold up to 64 responses in the queue */
+ if (atomic_read(&dev->response_available) > MBIM_MAX_RESP_Q) {
+ pr_err("Too much responses pending!\n");
+ mbim_unlock(&dev->write_excl);
+ return -EBUSY;
+ }
+
+ cpkt = mbim_alloc_ctrl_pkt(count, GFP_KERNEL);
+ if (!cpkt) {
+ pr_err("failed to allocate ctrl pkt\n");
+ mbim_unlock(&dev->write_excl);
+ return -ENOMEM;
+ }
+
+ cpkt->transaction_id = ctrl_msg->dwTransactionId;
+ ret = copy_from_user(cpkt->buf, buf, count);
+ if (ret) {
+ pr_err("copy_from_user failed err:%d\n", ret);
+ mbim_free_ctrl_pkt(cpkt);
+ mbim_unlock(&dev->write_excl);
+ return 0;
+ }
+
+ fmbim_send_cpkt_response(dev, cpkt);
+
+ mbim_unlock(&dev->write_excl);
+
+ return count;
+}
+
+static int mbim_open(struct inode *ip, struct file *fp)
+{
+ if (!_mbim_dev) {
+ pr_err("mbim_dev not created yet\n");
+ return -ENODEV;
+ }
+
+ if (_mbim_dev->mode != MODE_MBIM && _mbim_dev->mode != MODE_NCM_MBIM) {
+ pr_err("NOT mbim mode\n");
+ return -ENODEV;
+ }
+
+ if (mbim_lock(&_mbim_dev->open_excl)) {
+ pr_err("Already opened\n");
+ return -EBUSY;
+ }
+
+ if (!atomic_read(&_mbim_dev->online))
+ pr_info("MBIM not connected\n");
+
+ pr_debug("Lock mbim_dev->open_excl for open\n");
+
+ fp->private_data = _mbim_dev;
+
+ atomic_set(&_mbim_dev->error, 0);
+
+ // TODO - Check if needed
+ spin_lock(&_mbim_dev->lock);
+ _mbim_dev->is_open = true;
+ if (atomic_read(&_mbim_dev->online))
+ mbim_notify(_mbim_dev);
+ spin_unlock(&_mbim_dev->lock);
+
+ pr_debug("Exit, mbim file opened\n");
+
+ return 0;
+}
+
+static int mbim_release(struct inode *ip, struct file *fp)
+{
+ struct f_mbim *mbim = fp->private_data;
+ u8 max_loop = 10;
+
+ if (mbim->mode != MODE_MBIM && mbim->mode != MODE_NCM_MBIM) {
+ pr_err("NOT mbim mode\n");
+ return -ENODEV;
+ }
+
+ spin_lock(&mbim->lock);
+ mbim->is_open = false;
+ spin_unlock(&mbim->lock);
+
+ do {
+ spin_lock(&mbim->lock);
+ if (atomic_read(&mbim->response_available) == 0 &&
+ mbim->notify_req != NULL) {
+ spin_unlock(&mbim->lock);
+ break;
+ }
+
+ pr_err("in progress, waiting for all notifications to complete,"
+ "response_available = %d, notify_req = %p\n",
+ atomic_read(&mbim->response_available), mbim->notify_req);
+ spin_unlock(&mbim->lock);
+
+ if (--max_loop == 0) {
+ pr_err("Closing the file handler without notifying the host\n");
+ mbim_unlock(&_mbim_dev->open_excl);
+ return 0;
+ }
+
+ msleep(2000);
+ } while (1);
+
+ BUG_ON(!mbim->notify_req);
+ BUG_ON(atomic_read(&mbim->response_available));
+
+ /* add spinlock to protect the mbim_notify */
+ spin_lock(&mbim->lock);
+ if (atomic_read(&_mbim_dev->online))
+ mbim_notify(mbim);
+ spin_unlock(&mbim->lock);
+
+ mbim_unlock(&_mbim_dev->open_excl);
+
+ return 0;
+}
+
+static long mbim_ioctl(struct file *fp, unsigned cmd, unsigned long arg)
+{
+ struct f_mbim *mbim = fp->private_data;
+ int ret = 0;
+
+ pr_debug("Received command %d\n", cmd);
+
+ if (mbim->mode != MODE_MBIM && mbim->mode != MODE_NCM_MBIM) {
+ pr_err("NOT mbim mode\n");
+ return -ENODEV;
+ }
+
+ if (mbim_lock(&mbim->ioctl_excl))
+ return -EBUSY;
+
+ switch (cmd) {
+ case MBIM_GET_NTB_SIZE:
+ ret = copy_to_user((void __user *)arg,
+ &mbim->ntb_input_size, sizeof(mbim->ntb_input_size));
+ if (ret) {
+ pr_err("copying to user space failed\n");
+ ret = -EFAULT;
+ }
+ pr_info("Sent NTB size %d\n", mbim->ntb_input_size);
+ break;
+ case MBIM_GET_DATAGRAM_COUNT:
+ ret = copy_to_user((void __user *)arg,
+ &mbim->ntb_max_datagrams,
+ sizeof(mbim->ntb_max_datagrams));
+ if (ret) {
+ pr_err("copying to user space failed\n");
+ ret = -EFAULT;
+ }
+ pr_info("Sent NTB datagrams count %d\n",
+ mbim->ntb_max_datagrams);
+ break;
+ default:
+ pr_err("wrong parameter\n");
+ ret = -EINVAL;
+ }
+
+ mbim_unlock(&mbim->ioctl_excl);
+
+ return ret;
+}
+
+/* file operations for MBIM device /dev/mbim */
+static const struct file_operations mbim_fops = {
+ .owner = THIS_MODULE,
+ .open = mbim_open,
+ .release = mbim_release,
+ .read = mbim_read,
+ .write = mbim_write,
+ .unlocked_ioctl = mbim_ioctl,
+};
+
+static struct miscdevice mbim_device = {
+ .minor = MISC_DYNAMIC_MINOR,
+ .name = "mbim",
+ .fops = &mbim_fops,
+};
+
+static int mbim_init(int instances)
+{
+ int i;
+ struct f_mbim *dev = NULL;
+ int ret;
+
+ if (mbim_inited)
+ return 0;
+
+ pr_info("initialize %d instances\n", instances);
+
+ if (instances > NR_MBIM_PORTS) {
+ pr_err("Max-%d instances supported\n", NR_MBIM_PORTS);
+ return -EINVAL;
+ }
+
+ for (i = 0; i < instances; i++) {
+ dev = kzalloc(sizeof(struct f_mbim), GFP_KERNEL);
+ if (!dev) {
+ pr_err("Failed to allocate mbim dev\n");
+ ret = -ENOMEM;
+ goto fail_probe;
+ }
+
+ dev->port_num = i;
+ spin_lock_init(&dev->lock);
+ INIT_LIST_HEAD(&dev->cpkt_req_q);
+ INIT_LIST_HEAD(&dev->cpkt_resp_q);
+
+ mbim_ports[i].port = dev;
+ mbim_ports[i].port_num = i;
+
+ dev->mode = MODE_MBIM;
+ dev->state = STATE_MBIM;
+
+ init_waitqueue_head(&dev->read_wq);
+ init_waitqueue_head(&dev->write_wq);
+ init_waitqueue_head(&dev->open_wq);
+ atomic_set(&dev->open_excl, 0);
+ atomic_set(&dev->ioctl_excl, 0);
+ atomic_set(&dev->read_excl, 0);
+ atomic_set(&dev->write_excl, 0);
+ atomic_set(&dev->response_available, 0);
+
+ nr_mbim_ports++;
+
+ }
+
+ _mbim_dev = dev;
+ ret = misc_register(&mbim_device);
+ if (ret) {
+ pr_err("mbim driver failed to register\n");
+ goto fail_probe;
+ }
+
+ pr_info("Initialized %d ports\n", nr_mbim_ports);
+ mbim_inited = 1;
+ return ret;
+
+fail_probe:
+ pr_err("Failed\n");
+ for (i = 0; i < nr_mbim_ports; i++) {
+ kfree(mbim_ports[i].port);
+ mbim_ports[i].port = NULL;
+ }
+
+ return ret;
+}
+
+static void __maybe_unused fmbim_cleanup(void)
+{
+#if defined (ALLOW_MBIM_CLEANUP)
+ int i = 0;
+
+ for (i = 0; i < nr_mbim_ports; i++) {
+ /* to avoid memory leak */
+ mbim_clear_queues(mbim_ports[i].port);
+ kfree(mbim_ports[i].port);
+ mbim_ports[i].port = NULL;
+ }
+ nr_mbim_ports = 0;
+
+ misc_deregister(&mbim_device);
+ mbim_device.minor = MISC_DYNAMIC_MINOR;
+ _mbim_dev = NULL;
+#endif
+}