marvell/linux/drivers/usb/gadget/function/f_mbim.c

/*
 * 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 *) &ether_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 *) &ether_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 *) &ether_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 *) &ether_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 *) &ether_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 *) &ether_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 *) &ether_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 *) &ether_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 *) &ether_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 *) &ether_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 *) &ether_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 *) &ether_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
}