From 3b7ded42a25fc642354a3049328db484b4cc1426 Mon Sep 17 00:00:00 2001
From: Lunarixus <Lunarixus@raphielgang.org>
Date: Tue, 24 Mar 2020 21:41:25 +0000
Subject: [PATCH] drivers: usb: gadget: Revert to Nougat fixes

Signed-off-by: Lunarixus <Lunarixus@raphielgang.org>
---
 drivers/usb/gadget/android.c             |  53 ++-
 drivers/usb/gadget/composite.c           |  11 +-
 drivers/usb/gadget/configfs.c            |  22 +-
 drivers/usb/gadget/f_accessory.c         |  69 ++--
 drivers/usb/gadget/f_conn_gadget.ioctl.h |   1 +
 drivers/usb/gadget/f_fs.c                | 431 ++++++++++++++---------
 drivers/usb/gadget/f_mass_storage.c      |   6 +
 drivers/usb/gadget/f_mtp.c               |  52 ++-
 drivers/usb/gadget/f_mtp_samsung.c       |   5 +-
 drivers/usb/gadget/functions.c           |   2 +-
 drivers/usb/gadget/multi_config.c        |   3 -
 drivers/usb/gadget/multi_config.h        |   1 -
 drivers/usb/gadget/storage_common.c      |  37 ++
 include/uapi/linux/usb/functionfs.h      |  54 +--
 14 files changed, 428 insertions(+), 319 deletions(-)

diff --git a/drivers/usb/gadget/android.c b/drivers/usb/gadget/android.c
index 84ba1bbb5b9..c23bdf80522 100644
--- a/drivers/usb/gadget/android.c
+++ b/drivers/usb/gadget/android.c
@@ -211,7 +211,7 @@ static void android_work(struct work_struct *data)
 	char **uevent_envp = NULL;
 	unsigned long flags;
 
-	printk(KERN_DEBUG "usb: %s config=%pK,connected=%d,sw_connected=%d\n",
+	printk(KERN_DEBUG "usb: %s config=%p,connected=%d,sw_connected=%d\n",
 			__func__, cdev->config, dev->connected,
 			dev->sw_connected);
 	spin_lock_irqsave(&cdev->lock, flags);
@@ -230,7 +230,7 @@ static void android_work(struct work_struct *data)
 		printk(KERN_DEBUG "usb: %s sent uevent %s\n",
 			 __func__, uevent_envp[0]);
 	} else {
-		printk(KERN_DEBUG "usb: %s did not send uevent (%d %d %pK)\n",
+		printk(KERN_DEBUG "usb: %s did not send uevent (%d %d %p)\n",
 		 __func__, dev->connected, dev->sw_connected, cdev->config);
 	}
 }
@@ -268,6 +268,7 @@ struct functionfs_config {
 	bool opened;
 	bool enabled;
 	struct ffs_data *data;
+	struct android_dev *dev;
 };
 
 static int ffs_function_init(struct android_usb_function *f,
@@ -375,21 +376,32 @@ static int functionfs_ready_callback(struct ffs_data *ffs)
 	struct functionfs_config *config = ffs_function.config;
 	int ret = 0;
 
-	mutex_lock(&dev->mutex);
-
-	ret = functionfs_bind(ffs, dev->cdev);
-	if (ret)
-		goto err;
+	/* dev is null in case ADB is not in the composition */
+	if (dev) {
+		mutex_lock(&dev->mutex);
+		ret = functionfs_bind(ffs, dev->cdev);
+		if (ret) {
+			mutex_unlock(&dev->mutex);
+			return ret;
+		}
+	} else {
+		/* android ffs_func requires daemon to start only after enable*/
+		pr_debug("start adbd only in ADB composition\n");
+		return -ENODEV;
+	}
 
 	config->data = ffs;
 	config->opened = true;
+	/* Save dev in case the adb function will get disabled */
+	config->dev = dev;
 
 	if (config->enabled)
 		android_enable(dev);
 
-err:
 	mutex_unlock(&dev->mutex);
-	return ret;
+
+	return 0;
+
 }
 
 static void functionfs_closed_callback(struct ffs_data *ffs)
@@ -397,17 +409,32 @@ static void functionfs_closed_callback(struct ffs_data *ffs)
 	struct android_dev *dev = _android_dev;
 	struct functionfs_config *config = ffs_function.config;
 
-	mutex_lock(&dev->mutex);
+	/*
+	 * In case new composition is without ADB or ADB got disabled by the
+	 * time ffs_daemon was stopped then use saved one
+	 */
+	if (!dev)
+		dev = config->dev;
 
-	if (config->enabled)
+	/* fatal-error: It should never happen */
+	if (!dev)
+		pr_err("adb_closed_callback: config->dev is NULL");
+
+	if (dev)
+		mutex_lock(&dev->mutex);
+
+	if (config->enabled && dev)
 		android_disable(dev);
 
+	config->dev = NULL;
+
 	config->opened = false;
 	config->data = NULL;
 
 	functionfs_unbind(ffs);
 
-	mutex_unlock(&dev->mutex);
+	if (dev)
+		mutex_unlock(&dev->mutex);
 }
 
 static void *functionfs_acquire_dev_callback(const char *dev_name)
@@ -1808,8 +1835,6 @@ functions_store(struct device *pdev, struct device_attribute *attr,
 #endif
 	while (b) {
 		name = strsep(&b, ",");
-		if (!name)
-			continue;
 
 		is_ffs = 0;
 		strlcpy(aliases, dev->ffs_aliases, sizeof(aliases));
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index 626188c810c..bb7d70cdc1e 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -638,15 +638,6 @@ static int set_config(struct usb_composite_dev *cdev,
 	unsigned		power = gadget_is_otg(gadget) ? 8 : 100;
 	int			tmp;
 
-	/*
-	 * ignore if SET_CONFIGURATION
-	 * is sent again for same config value.
-	 */
-	if (cdev->config && (cdev->config->bConfigurationValue == number)) {
-		DBG(cdev, "already in the same config with value %d\n",
-				number);
-		return 0;
-	}
 	if (number) {
 		list_for_each_entry(c, &cdev->configs, list) {
 #ifdef CONFIG_USB_ANDROID_SAMSUNG_COMPOSITE
@@ -1333,7 +1324,7 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 					cdev->desc.bcdUSB = cpu_to_le16(0x0300);
 					cdev->desc.bMaxPacketSize0 = 9;
 				} else {
-					cdev->desc.bcdUSB = cpu_to_le16(0x0210);
+					cdev->desc.bcdUSB = cpu_to_le16(0x0201);
 				}
 			}
 
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index 5a65722d16d..47fd42cce12 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -87,28 +87,21 @@ struct gadget_config_name {
 	struct list_head list;
 };
 
-#define MAX_USB_STRING_LEN	126
-#define MAX_USB_STRING_WITH_NULL_LEN	(MAX_USB_STRING_LEN+1)
-
 static int usb_string_copy(const char *s, char **s_copy)
 {
 	int ret;
 	char *str;
 	char *copy = *s_copy;
 	ret = strlen(s);
-	if (ret > MAX_USB_STRING_LEN)
+	if (ret > 126)
 		return -EOVERFLOW;
 
-	if (copy) {
-		str = copy;
-	} else {
-		str = kmalloc(MAX_USB_STRING_WITH_NULL_LEN, GFP_KERNEL);
-		if (!str)
-			return -ENOMEM;
-	}
-	strncpy(str, s, MAX_USB_STRING_WITH_NULL_LEN);
+	str = kstrdup(s, GFP_KERNEL);
+	if (!str)
+		return -ENOMEM;
 	if (str[ret - 1] == '\n')
 		str[ret - 1] = '\0';
+	kfree(copy);
 	*s_copy = str;
 	return 0;
 }
@@ -400,6 +393,11 @@ static int config_usb_cfg_link(
 	}
 
 	f = usb_get_function(fi);
+	if (f == NULL) {
+		/* Are we trying to symlink PTP without MTP function? */
+		ret = -EINVAL; /* Invalid Configuration */
+		goto out;
+	}
 	if (IS_ERR(f)) {
 		ret = PTR_ERR(f);
 		goto out;
diff --git a/drivers/usb/gadget/f_accessory.c b/drivers/usb/gadget/f_accessory.c
index 968d37ab305..059979d54cd 100644
--- a/drivers/usb/gadget/f_accessory.c
+++ b/drivers/usb/gadget/f_accessory.c
@@ -256,6 +256,7 @@ static struct usb_request *req_get(struct acc_dev *dev, struct list_head *head)
 
 static void acc_set_disconnected(struct acc_dev *dev)
 {
+	dev->online = 0;
 	dev->disconnected = 1;
 }
 
@@ -504,7 +505,7 @@ static int create_bulk_endpoints(struct acc_dev *dev,
 	struct usb_ep *ep;
 	int i;
 
-	DBG(cdev, "create_bulk_endpoints dev: %pK\n", dev);
+	DBG(cdev, "create_bulk_endpoints dev: %p\n", dev);
 
 	ep = usb_ep_autoconfig(cdev->gadget, in_desc);
 	if (!ep) {
@@ -524,7 +525,15 @@ static int create_bulk_endpoints(struct acc_dev *dev,
 	ep->driver_data = dev;		/* claim the endpoint */
 	dev->ep_out = ep;
 
-	
+	ep = usb_ep_autoconfig(cdev->gadget, out_desc);
+	if (!ep) {
+		DBG(cdev, "usb_ep_autoconfig for ep_out failed\n");
+		return -ENODEV;
+	}
+	DBG(cdev, "usb_ep_autoconfig for ep_out got %s\n", ep->name);
+	ep->driver_data = dev;		/* claim the endpoint */
+	dev->ep_out = ep;
+
 	/* now allocate requests for our endpoints */
 	for (i = 0; i < TX_REQ_MAX; i++) {
 		req = acc_request_new(dev->ep_in, BULK_BUFFER_SIZE);
@@ -559,7 +568,6 @@ static ssize_t acc_read(struct file *fp, char __user *buf,
 	struct usb_request *req;
 	ssize_t r = count;
 	unsigned xfer;
-	int len;
 	int ret = 0;
 
 	pr_debug("acc_read(%zu)\n", count);
@@ -580,8 +588,6 @@ static ssize_t acc_read(struct file *fp, char __user *buf,
 		goto done;
 	}
 
-	len = ALIGN(count, dev->ep_out->maxpacket);
-	
 	if (dev->rx_done) {
 		// last req cancelled. try to get it.
 		req = dev->rx_req[0];
@@ -591,14 +597,14 @@ static ssize_t acc_read(struct file *fp, char __user *buf,
 requeue_req:
 	/* queue a request */
 	req = dev->rx_req[0];
-	req->length = len;
+	req->length = count;
 	dev->rx_done = 0;
 	ret = usb_ep_queue(dev->ep_out, req, GFP_KERNEL);
 	if (ret < 0) {
 		r = -EIO;
 		goto done;
 	} else {
-		pr_debug("rx %pK queue\n", req);
+		pr_debug("rx %p queue\n", req);
 	}
 
 	/* wait for a request to complete */
@@ -621,7 +627,7 @@ static ssize_t acc_read(struct file *fp, char __user *buf,
 		if (req->actual == 0)
 			goto requeue_req;
 
-		pr_debug("rx %pK %u\n", req, req->actual);
+		pr_debug("rx %p %u\n", req, req->actual);
 		xfer = (req->actual < count) ? req->actual : count;
 		r = xfer;
 		if (copy_to_user(buf, req->buf, xfer))
@@ -745,11 +751,9 @@ static long acc_ioctl(struct file *fp, unsigned code, unsigned long value)
 
 static int acc_open(struct inode *ip, struct file *fp)
 {
-	if (atomic_xchg(&_acc_dev->open_excl, 1)) {
-		printk(KERN_INFO "usb: acc_open_EBUSY\n");
+	printk(KERN_INFO "acc_open\n");
+	if (atomic_xchg(&_acc_dev->open_excl, 1))
 		return -EBUSY;
-	}
-	printk(KERN_INFO "usb: acc_open\n");
 
 	_acc_dev->disconnected = 0;
 	fp->private_data = _acc_dev;
@@ -761,10 +765,7 @@ static int acc_release(struct inode *ip, struct file *fp)
 	printk(KERN_INFO "acc_release\n");
 
 	WARN_ON(!atomic_xchg(&_acc_dev->open_excl, 0));
-	/* indicate that we are disconnected
-	 * still could be online so don't touch online flag
-	 */
-	_acc_dev->disconnected = 1;
+	_acc_dev->disconnected = 0;
 	return 0;
 }
 
@@ -774,9 +775,6 @@ static const struct file_operations acc_fops = {
 	.read = acc_read,
 	.write = acc_write,
 	.unlocked_ioctl = acc_ioctl,
-#ifdef CONFIG_COMPAT
-	.compat_ioctl = acc_ioctl,
-#endif
 	.open = acc_open,
 	.release = acc_release,
 };
@@ -792,14 +790,6 @@ static int acc_hid_probe(struct hid_device *hdev,
 	return hid_hw_start(hdev, HID_CONNECT_DEFAULT);
 }
 
-static void acc_complete_setup_noop(struct usb_ep *ep, struct usb_request *req)
-{
-	/*
-	 * Default no-op function when nothing needs to be done for the
-	 * setup request
-	 */
-}
-
 static struct miscdevice acc_device = {
 	.minor = MISC_DYNAMIC_MINOR,
 	.name = "usb_accessory",
@@ -845,7 +835,6 @@ static int acc_ctrlrequest(struct usb_composite_dev *cdev,
 			schedule_delayed_work(
 				&dev->start_work, msecs_to_jiffies(10));
 			value = 0;
-			cdev->req->complete = acc_complete_setup_noop;
 		} else if (b_request == ACCESSORY_SEND_STRING) {
 			dev->string_index = w_index;
 			cdev->gadget->ep0->driver_data = dev;
@@ -854,13 +843,10 @@ static int acc_ctrlrequest(struct usb_composite_dev *cdev,
 		} else if (b_request == ACCESSORY_SET_AUDIO_MODE &&
 				w_index == 0 && w_length == 0) {
 			dev->audio_mode = w_value;
-			cdev->req->complete = acc_complete_setup_noop;
 			value = 0;
 		} else if (b_request == ACCESSORY_REGISTER_HID) {
-			cdev->req->complete = acc_complete_setup_noop;
 			value = acc_register_hid(dev, w_value, w_index);
 		} else if (b_request == ACCESSORY_UNREGISTER_HID) {
-			cdev->req->complete = acc_complete_setup_noop;
 			value = acc_unregister_hid(dev, w_value);
 		} else if (b_request == ACCESSORY_SET_HID_REPORT_DESC) {
 			spin_lock_irqsave(&dev->lock, flags);
@@ -895,8 +881,7 @@ static int acc_ctrlrequest(struct usb_composite_dev *cdev,
 		if (b_request == ACCESSORY_GET_PROTOCOL) {
 			*((u16 *)cdev->req->buf) = PROTOCOL_VERSION;
 			value = sizeof(u16);
-			
-			cdev->req->complete = acc_complete_setup_noop;
+
 			/* clear any string left over from a previous session */
 			memset(dev->manufacturer, 0, sizeof(dev->manufacturer));
 			memset(dev->model, 0, sizeof(dev->model));
@@ -906,8 +891,6 @@ static int acc_ctrlrequest(struct usb_composite_dev *cdev,
 			memset(dev->serial, 0, sizeof(dev->serial));
 			dev->start_requested = 0;
 			dev->audio_mode = 0;
-			strlcpy(dev->manufacturer, "Android", ACC_STRING_SIZE);
-			strlcpy(dev->model, "Android", ACC_STRING_SIZE);		
 		}
 	}
 
@@ -938,7 +921,7 @@ acc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	int			id;
 	int			ret;
 
-	DBG(cdev, "acc_function_bind dev: %pK\n", dev);
+	DBG(cdev, "acc_function_bind dev: %p\n", dev);
 
 	ret = hid_register_driver(&acc_hid_driver);
 	if (ret)
@@ -1009,10 +992,6 @@ acc_function_unbind(struct usb_configuration *c, struct usb_function *f)
 	struct usb_request *req;
 	int i;
 
-	dev->online = 0;		/* clear online flag */
-	wake_up(&dev->read_wq);		/* unblock reads on closure */
-	wake_up(&dev->write_wq);	/* likewise for writes */
-
 	while ((req = req_get(dev, &dev->tx_idle)))
 		acc_request_free(req, dev->ep_in);
 	for (i = 0; i < RX_REQ_MAX; i++)
@@ -1024,7 +1003,6 @@ acc_function_unbind(struct usb_configuration *c, struct usb_function *f)
 static void acc_start_work(struct work_struct *data)
 {
 	char *envp[2] = { "ACCESSORY=START", NULL };
-	printk(KERN_INFO "usb: Send uevent, ACCESSORY=START\n");
 	kobject_uevent_env(&acc_device.this_device->kobj, KOBJ_CHANGE, envp);
 #ifdef CONFIG_USB_NOTIFY_PROC_LOG
 	store_usblog_notify(NOTIFY_USBSTATE, (void *)envp[0], NULL);
@@ -1100,7 +1078,7 @@ static void acc_hid_work(struct work_struct *data)
 	list_for_each_safe(entry, temp, &new_list) {
 		hid = list_entry(entry, struct acc_hid_dev, list);
 		if (acc_hid_init(hid)) {
-			pr_err("can't add HID device %pK\n", hid);
+			pr_err("can't add HID device %p\n", hid);
 			acc_hid_delete(hid);
 		} else {
 			spin_lock_irqsave(&dev->lock, flags);
@@ -1147,7 +1125,6 @@ static int acc_function_set_alt(struct usb_function *f,
 	}
 
 	dev->online = 1;
-	dev->disconnected = 0; /* if online then not disconnected */
 
 	/* readers may be blocked waiting for us to go online */
 	wake_up(&dev->read_wq);
@@ -1161,7 +1138,6 @@ static void acc_function_disable(struct usb_function *f)
 
 	DBG(cdev, "acc_function_disable\n");
 	acc_set_disconnected(dev);
-	dev->online = 0; /* so now need to clear online flag here too */
 	usb_ep_disable(dev->ep_in);
 	usb_ep_disable(dev->ep_out);
 
@@ -1220,13 +1196,12 @@ static int acc_setup(void)
 	INIT_DELAYED_WORK(&dev->start_work, acc_start_work);
 	INIT_WORK(&dev->hid_work, acc_hid_work);
 
+	/* _acc_dev must be set before calling usb_gadget_register_driver */
+	_acc_dev = dev;
 
 	ret = misc_register(&acc_device);
 	if (ret)
 		goto err;
-	
-	/* _acc_dev must be set before calling usb_gadget_register_driver */
-	_acc_dev = dev;
 
 	return 0;
 
diff --git a/drivers/usb/gadget/f_conn_gadget.ioctl.h b/drivers/usb/gadget/f_conn_gadget.ioctl.h
index 92069b2c1ea..bd739545393 100644
--- a/drivers/usb/gadget/f_conn_gadget.ioctl.h
+++ b/drivers/usb/gadget/f_conn_gadget.ioctl.h
@@ -40,4 +40,5 @@ enum {
 #define CONN_GADGET_IOCTL_BIND_WAIT_NOTIFY	_IOR(CONN_GADGET_IOCTL_MAGIC_SIG, CONN_GADGET_IOCTL_NR_1, int)
 #define CONN_GADGET_IOCTL_BIND_GET_STATUS	_IOR(CONN_GADGET_IOCTL_MAGIC_SIG, CONN_GADGET_IOCTL_NR_2, int)
 #define CONN_GADGET_IOCTL_MAX_NR			CONN_GADGET_IOCTL_NR_MAX
+
 #endif
diff --git a/drivers/usb/gadget/f_fs.c b/drivers/usb/gadget/f_fs.c
index cf2ba51e4b1..cabf11f2d5b 100644
--- a/drivers/usb/gadget/f_fs.c
+++ b/drivers/usb/gadget/f_fs.c
@@ -140,6 +140,9 @@ struct ffs_data {
 	 */
 	struct usb_request		*ep0req;		/* P: mutex */
 	struct completion		ep0req_completion;	/* P: mutex */
+	int				ep0req_status;		/* P: mutex */
+	struct completion		epin_completion;
+	struct completion		epout_completion;
 
 	/* reference counter */
 	atomic_t			ref;
@@ -193,13 +196,16 @@ struct ffs_data {
 
 	/* filled by __ffs_data_got_descs() */
 	/*
-	 * raw_descs is what you kfree, real_descs points inside of raw_descs,
-	 * where full speed, high speed and super speed descriptors start.
-	 * real_descs_length is the length of all those descriptors.
+	 * Real descriptors are 16 bytes after raw_descs (so you need
+	 * to skip 16 bytes (ie. ffs->raw_descs + 16) to get to the
+	 * first full speed descriptor).  raw_descs_length and
+	 * raw_fs_descs_length do not have those 16 bytes added.
 	 */
-	const void			*raw_descs_data;
 	const void			*raw_descs;
 	unsigned			raw_descs_length;
+	unsigned			raw_fs_hs_descs_length;
+	unsigned			raw_ss_descs_offset;
+	unsigned			raw_ss_descs_length;
 	unsigned			fs_descs_count;
 	unsigned			hs_descs_count;
 	unsigned			ss_descs_count;
@@ -312,6 +318,7 @@ struct ffs_epfile {
 	/* Protects ep->ep and ep->req. */
 	struct mutex			mutex;
 	wait_queue_head_t		wait;
+	atomic_t			error;
 
 	struct ffs_data			*ffs;
 	struct ffs_ep			*ep;	/* P: ffs->eps_lock */
@@ -385,7 +392,7 @@ static int __ffs_ep0_queue_wait(struct ffs_data *ffs, char *data, size_t len)
 	}
 
 	ffs->setup_state = FFS_NO_SETUP;
-	return req->status ? req->status : req->actual;
+	return ffs->ep0req_status;
 }
 
 static int __ffs_ep0_stall(struct ffs_data *ffs)
@@ -459,6 +466,13 @@ static ssize_t ffs_ep0_write(struct file *file, const char __user *buf,
 			ffs->state = FFS_ACTIVE;
 			mutex_unlock(&ffs->mutex);
 
+			ret = functionfs_ready_callback(ffs);
+			if (unlikely(ret < 0)) {
+				ffs->state = FFS_CLOSING;
+				return ret;
+			}
+
+			set_bit(FFS_FL_CALL_CLOSED_CALLBACK, &ffs->flags);
 			return len;
 		}
 		break;
@@ -733,8 +747,11 @@ static const struct file_operations ffs_ep0_operations = {
 
 static void ffs_epfile_io_complete(struct usb_ep *_ep, struct usb_request *req)
 {
+	struct ffs_ep *ep = _ep->driver_data;
 	ENTER();
-	if (likely(req->context)) {
+
+	/* req may be freed during unbind */
+	if (ep && ep->req && likely(req->context)) {
 		struct ffs_ep *ep = _ep->driver_data;
 		ep->status = req->status ? req->status : req->actual;
 		complete(req->context);
@@ -745,108 +762,152 @@ static ssize_t ffs_epfile_io(struct file *file,
 			     char __user *buf, size_t len, int read)
 {
 	struct ffs_epfile *epfile = file->private_data;
-	struct usb_gadget *gadget = epfile->ffs->gadget;
 	struct ffs_ep *ep;
+	struct ffs_data *ffs = epfile->ffs;
 	char *data = NULL;
-	ssize_t ret, data_len;
+	ssize_t ret;
 	int halt;
+	int buffer_len = !read ? len : round_up(len, 1024);
 
-	/* Are we still active? */
-	if (WARN_ON(epfile->ffs->state != FFS_ACTIVE)) {
-		ret = -ENODEV;
-		goto error;
-	}
+	pr_debug("%s: len %ld, buffer_len %d, read %d\n", __func__, len, buffer_len, read);
 
-	/* Wait for endpoint to be enabled */
-	ep = epfile->ep;
-	if (!ep) {
-		if (file->f_flags & O_NONBLOCK) {
-			ret = -EAGAIN;
-			goto error;
-		}
+	if (atomic_read(&epfile->error))
+		return -ENODEV;
 
-		ret = wait_event_interruptible(epfile->wait, (ep = epfile->ep));
-		if (ret) {
-			ret = -EINTR;
+	goto first_try;
+	do {
+		spin_unlock_irq(&epfile->ffs->eps_lock);
+		mutex_unlock(&epfile->mutex);
+
+first_try:
+		/* Are we still active? */
+		if (WARN_ON(epfile->ffs->state != FFS_ACTIVE)) {
+			ret = -ENODEV;
 			goto error;
 		}
-	}
 
-	/* Do we halt? */
-	halt = !read == !epfile->in;
-	if (halt && epfile->isoc) {
-		ret = -EINVAL;
-		goto error;
-	}
+		/* Wait for endpoint to be enabled */
+		ep = epfile->ep;
+		if (!ep) {
+			if (file->f_flags & O_NONBLOCK) {
+				ret = -EAGAIN;
+				goto error;
+			}
 
-	/* Allocate & copy */
-	if (!halt) {
-		/*
-		 * Controller may require buffer size to be aligned to
-		 * maxpacketsize of an out endpoint.
-		 */
-		data_len = read ? usb_ep_align_maybe(gadget, ep->ep, len) : len;
+			/* Don't wait on write if device is offline */
+			if (!read) {
+				ret = -ENODEV;
+				goto error;
+			}
 
-		data = kmalloc(data_len, GFP_KERNEL);
-		if (unlikely(!data))
-			return -ENOMEM;
+			/*
+			 * if ep is disabled, this fails all current IOs
+			 * and wait for next epfile open to happen
+			 */
+			if (!atomic_read(&epfile->error)) {
+				ret = wait_event_interruptible(epfile->wait,
+					(ep = epfile->ep));
+				if (ret < 0)
+					goto error;
+			}
+			if (!ep) {
+				ret = -ENODEV;
+				goto error;
+			}
+		}
 
-		if (!read && unlikely(copy_from_user(data, buf, len))) {
-			ret = -EFAULT;
+		/* Do we halt? */
+		halt = !read == !epfile->in;
+		if (halt && epfile->isoc) {
+			ret = -EINVAL;
 			goto error;
 		}
-	}
 
-	/* We will be using request */
-	ret = ffs_mutex_lock(&epfile->mutex, file->f_flags & O_NONBLOCK);
-	if (unlikely(ret))
-		goto error;
+		/* Allocate & copy */
+		if (!halt && !data) {
+			data = kzalloc(buffer_len, GFP_KERNEL);
+			if (unlikely(!data))
+				return -ENOMEM;
 
-	spin_lock_irq(&epfile->ffs->eps_lock);
+			if (!read &&
+			    unlikely(__copy_from_user(data, buf, len))) {
+				ret = -EFAULT;
+				goto error;
+			}
+		}
 
-	if (epfile->ep != ep) {
-		/* In the meantime, endpoint got disabled or changed. */
-		ret = -ESHUTDOWN;
-		spin_unlock_irq(&epfile->ffs->eps_lock);
-	} else if (halt) {
-		/* Halt */
+		/* We will be using request */
+		ret = ffs_mutex_lock(&epfile->mutex,
+				     file->f_flags & O_NONBLOCK);
+		if (unlikely(ret))
+			goto error;
+
+		/*
+		 * We're called from user space, we can use _irq rather then
+		 * _irqsave
+		 */
+		spin_lock_irq(&epfile->ffs->eps_lock);
+
+		/*
+		 * While we were acquiring mutex endpoint got disabled
+		 * or changed?
+		 */
+	} while (unlikely(epfile->ep != ep));
+
+	/* Halt */
+	if (unlikely(halt)) {
 		if (likely(epfile->ep == ep) && !WARN_ON(!ep->ep))
 			usb_ep_set_halt(ep->ep);
 		spin_unlock_irq(&epfile->ffs->eps_lock);
 		ret = -EBADMSG;
 	} else {
 		/* Fire the request */
-		DECLARE_COMPLETION_ONSTACK(done);
+		struct completion *done;
 
 		struct usb_request *req = ep->req;
-		req->context  = &done;
 		req->complete = ffs_epfile_io_complete;
 		req->buf      = data;
-		req->length   = data_len;
+		req->length   = buffer_len;
+
+		if (read) {
+			INIT_COMPLETION(ffs->epout_completion);
+			req->context  = done = &ffs->epout_completion;
+		} else {
+			INIT_COMPLETION(ffs->epin_completion);
+			req->context  = done = &ffs->epin_completion;
+		}
 
 		ret = usb_ep_queue(ep->ep, req, GFP_ATOMIC);
 
 		spin_unlock_irq(&epfile->ffs->eps_lock);
 
 		if (unlikely(ret < 0)) {
-			/* nop */
-		} else if (unlikely(wait_for_completion_interruptible(&done))) {
+			ret = -EIO;
+		} else if (unlikely(wait_for_completion_interruptible(done))) {
+			spin_lock_irq(&epfile->ffs->eps_lock);
+			/*
+			 * While we were acquiring lock endpoint got disabled
+			 * (disconnect) or changed (composition switch) ?
+			 */
+			if (epfile->ep == ep)
+				usb_ep_dequeue(ep->ep, req);
+			spin_unlock_irq(&epfile->ffs->eps_lock);
 			ret = -EINTR;
-			usb_ep_dequeue(ep->ep, req);
 		} else {
+			spin_lock_irq(&epfile->ffs->eps_lock);
 			/*
-			 * XXX We may end up silently droping data
-			 * here.  Since data_len (i.e. req->length) may
-			 * be bigger than len (after being rounded up
-			 * to maxpacketsize), we may end up with more
-			 * data then user space has space for.
+			 * While we were acquiring lock endpoint got disabled
+			 * (disconnect) or changed (composition switch) ?
 			 */
-			ret = ep->status;
+			if (epfile->ep == ep)
+				ret = ep->status;
+			else
+				ret = -ENODEV;
+			spin_unlock_irq(&epfile->ffs->eps_lock);
 			if (read && ret > 0) {
-				ret = min_t(size_t, ret, len);
-
-				if (unlikely(copy_to_user(buf,
-					data, ret)))
+				if (ret > len)
+					ret = -EOVERFLOW;
+				else if (unlikely(copy_to_user(buf, data, ret)))
 					ret = -EFAULT;
 			}
 		}
@@ -887,18 +948,7 @@ ffs_epfile_open(struct inode *inode, struct file *file)
 
 	file->private_data = epfile;
 	ffs_data_opened(epfile->ffs);
-
-	/* if all endpoints including EP0 (+ 1) opened ... */
-	if (atomic_read(&epfile->ffs->opened) >= epfile->ffs->eps_count + 1) {
-		if (!test_and_set_bit(FFS_FL_CALL_CLOSED_CALLBACK,
-					&epfile->ffs->flags)) {
-			pr_info("functionfs is ready\n");
-			mdelay(20);
-			/* REVISIT: what shall we do if the callback fails? */
-			if (functionfs_ready_callback(epfile->ffs) < 0)
-				pr_warn("functionfs ready callback failed\n");
-		}
-	}
+	atomic_set(&epfile->error, 0);
 
 	return 0;
 }
@@ -910,7 +960,9 @@ ffs_epfile_release(struct inode *inode, struct file *file)
 
 	ENTER();
 
+	atomic_set(&epfile->error, 1);
 	ffs_data_closed(epfile->ffs);
+	file->private_data = NULL;
 
 	return 0;
 }
@@ -1305,13 +1357,6 @@ static void ffs_data_closed(struct ffs_data *ffs)
 		ffs_data_reset(ffs);
 	}
 
-	/* call closed callback even if only one file is closed */
-	if (test_and_clear_bit(FFS_FL_CALL_CLOSED_CALLBACK, &ffs->flags)) {
-		pr_info("functionfs closed added delay\n");
-		mdelay(20);
-		functionfs_closed_callback(ffs);
-	}
-
 	ffs_data_put(ffs);
 }
 
@@ -1330,6 +1375,8 @@ static struct ffs_data *ffs_data_new(void)
 	spin_lock_init(&ffs->eps_lock);
 	init_waitqueue_head(&ffs->ev.waitq);
 	init_completion(&ffs->ep0req_completion);
+	init_completion(&ffs->epout_completion);
+	init_completion(&ffs->epin_completion);
 
 	/* XXX REVISIT need to update it in some places, or do we? */
 	ffs->ev.can_stall = 1;
@@ -1341,12 +1388,15 @@ static void ffs_data_clear(struct ffs_data *ffs)
 {
 	ENTER();
 
+	if (test_and_clear_bit(FFS_FL_CALL_CLOSED_CALLBACK, &ffs->flags))
+		functionfs_closed_callback(ffs);
+
 	BUG_ON(ffs->gadget);
 
 	if (ffs->epfiles)
 		ffs_epfiles_destroy(ffs->epfiles, ffs->eps_count);
 
-	kfree(ffs->raw_descs_data);
+	kfree(ffs->raw_descs);
 	kfree(ffs->raw_strings);
 	kfree(ffs->stringtabs);
 }
@@ -1358,12 +1408,14 @@ static void ffs_data_reset(struct ffs_data *ffs)
 	ffs_data_clear(ffs);
 
 	ffs->epfiles = NULL;
-	ffs->raw_descs_data = NULL;
 	ffs->raw_descs = NULL;
 	ffs->raw_strings = NULL;
 	ffs->stringtabs = NULL;
 
 	ffs->raw_descs_length = 0;
+	ffs->raw_fs_hs_descs_length = 0;
+	ffs->raw_ss_descs_offset = 0;
+	ffs->raw_ss_descs_length = 0;
 	ffs->fs_descs_count = 0;
 	ffs->hs_descs_count = 0;
 	ffs->ss_descs_count = 0;
@@ -1424,8 +1476,8 @@ static void functionfs_unbind(struct ffs_data *ffs)
 		usb_ep_free_request(ffs->gadget->ep0, ffs->ep0req);
 		ffs->ep0req = NULL;
 		ffs->gadget = NULL;
-		clear_bit(FFS_FL_BOUND, &ffs->flags);
 		ffs_data_put(ffs);
+		clear_bit(FFS_FL_BOUND, &ffs->flags);
 	}
 }
 
@@ -1528,6 +1580,7 @@ static void ffs_func_free(struct ffs_function *func)
 		if (ep->ep && ep->req)
 			usb_ep_free_request(ep->ep, ep->req);
 		ep->req = NULL;
+		ep->ep = NULL;
 		++ep;
 	} while (--count);
 	spin_unlock_irqrestore(&func->ffs->eps_lock, flags);
@@ -1553,9 +1606,12 @@ static void ffs_func_eps_disable(struct ffs_function *func)
 
 	spin_lock_irqsave(&func->ffs->eps_lock, flags);
 	do {
+		atomic_set(&epfile->error, 1);
 		/* pending requests get nuked */
-		if (likely(ep->ep))
+		if (likely(ep->ep)) {
 			usb_ep_disable(ep->ep);
+			ep->ep->driver_data = NULL;
+		}
 		epfile->ep = NULL;
 
 		++ep;
@@ -1585,11 +1641,7 @@ static int ffs_func_eps_enable(struct ffs_function *func)
 		else
 			desc_idx = 0;
 
-		/* fall-back to lower speed if desc missing for current speed */
-		do {
-			ds = ep->descs[desc_idx];
-		} while (!ds && --desc_idx >= 0);
-
+		ds = ep->descs[desc_idx];
 		if (!ds) {
 			ret = -EINVAL;
 			break;
@@ -1844,76 +1896,96 @@ static int __ffs_data_do_entity(enum ffs_entity_type type,
 static int __ffs_data_got_descs(struct ffs_data *ffs,
 				char *const _data, size_t len)
 {
-	char *data = _data, *raw_descs;
-	unsigned counts[3], flags;
-	int ret = -EINVAL, i;
+	unsigned fs_count, hs_count, ss_count = 0;
+	int fs_len, hs_len, ss_len, ss_magic, ret = -EINVAL;
+	char *data = _data;
 
 	ENTER();
 
-	if (get_unaligned_le32(data + 4) != len)
+	if (unlikely(get_unaligned_le32(data) != FUNCTIONFS_DESCRIPTORS_MAGIC ||
+		     get_unaligned_le32(data + 4) != len))
 		goto error;
+	fs_count = get_unaligned_le32(data +  8);
+	hs_count = get_unaligned_le32(data + 12);
 
-	switch (get_unaligned_le32(data)) {
-	case FUNCTIONFS_DESCRIPTORS_MAGIC:
-		flags = FUNCTIONFS_HAS_FS_DESC | FUNCTIONFS_HAS_HS_DESC;
-		data += 8;
-		len  -= 8;
-		break;
-	case FUNCTIONFS_DESCRIPTORS_MAGIC_V2:
-		flags = get_unaligned_le32(data + 8);
-		if (flags & ~(FUNCTIONFS_HAS_FS_DESC |
-			      FUNCTIONFS_HAS_HS_DESC |
-			      FUNCTIONFS_HAS_SS_DESC)) {
-			ret = -ENOSYS;
+	if (!fs_count && !hs_count)
+		goto einval;
+
+	data += 16;
+	len  -= 16;
+
+	if (likely(fs_count)) {
+		fs_len = ffs_do_descs(fs_count, data, len,
+				      __ffs_data_do_entity, ffs);
+		if (unlikely(fs_len < 0)) {
+			ret = fs_len;
 			goto error;
 		}
-		data += 12;
-		len  -= 12;
-		break;
-	default:
-		goto error;
+
+		data += fs_len;
+		len  -= fs_len;
+	} else {
+		fs_len = 0;
 	}
 
-	/* Read fs_count, hs_count and ss_count (if present) */
-	for (i = 0; i < 3; ++i) {
-		if (!(flags & (1 << i))) {
-			counts[i] = 0;
-		} else if (len < 4) {
+	if (likely(hs_count)) {
+		hs_len = ffs_do_descs(hs_count, data, len,
+				   __ffs_data_do_entity, ffs);
+		if (unlikely(hs_len < 0)) {
+			ret = hs_len;
 			goto error;
-		} else {
-			counts[i] = get_unaligned_le32(data);
-			data += 4;
-			len  -= 4;
 		}
+	} else {
+		hs_len = 0;
+	}
+
+	if ((len >= hs_len + 8)) {
+		/* Check SS_MAGIC for presence of ss_descs and get SS_COUNT */
+		ss_magic = get_unaligned_le32(data + hs_len);
+		if (ss_magic != FUNCTIONFS_SS_DESC_MAGIC)
+			goto einval;
+
+		ss_count = get_unaligned_le32(data + hs_len + 4);
+		data += hs_len + 8;
+		len  -= hs_len + 8;
+	} else {
+		data += hs_len;
+		len  -= hs_len;
 	}
 
-	/* Read descriptors */
-	raw_descs = data;
-	for (i = 0; i < 3; ++i) {
-		if (!counts[i])
-			continue;
-		ret = ffs_do_descs(counts[i], data, len,
+	if (!fs_count && !hs_count && !ss_count)
+		goto einval;
+
+	if (ss_count) {
+		ss_len = ffs_do_descs(ss_count, data, len,
 				   __ffs_data_do_entity, ffs);
-		if (ret < 0)
+		if (unlikely(ss_len < 0)) {
+			ret = ss_len;
 			goto error;
-		data += ret;
-		len  -= ret;
+		}
+		ret = ss_len;
+	} else {
+		ss_len = 0;
+		ret = 0;
 	}
 
-	if (raw_descs == data || len) {
-		ret = -EINVAL;
-		goto error;
-	}
+	if (unlikely(len != ret))
+		goto einval;
 
-	ffs->raw_descs_data	= _data;
-	ffs->raw_descs		= raw_descs;
-	ffs->raw_descs_length	= data - raw_descs;
-	ffs->fs_descs_count	= counts[0];
-	ffs->hs_descs_count	= counts[1];
-	ffs->ss_descs_count	= counts[2];
+	ffs->raw_fs_hs_descs_length	 = fs_len + hs_len;
+	ffs->raw_ss_descs_length	 = ss_len;
+	ffs->raw_descs_length		 = ffs->raw_fs_hs_descs_length + ss_len;
+	ffs->raw_descs			 = _data;
+	ffs->fs_descs_count		 = fs_count;
+	ffs->hs_descs_count		 = hs_count;
+	ffs->ss_descs_count		 = ss_count;
+	if (ffs->ss_descs_count)
+		ffs->raw_ss_descs_offset = 16 + ffs->raw_fs_hs_descs_length + 8;
 
 	return 0;
 
+einval:
+	ret = -EINVAL;
 error:
 	kfree(_data);
 	return ret;
@@ -2127,27 +2199,27 @@ static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
 	struct usb_endpoint_descriptor *ds = (void *)desc;
 	struct ffs_function *func = priv;
 	struct ffs_ep *ffs_ep;
+
+	/*
+	 * If hs_descriptors is not NULL then we are reading hs
+	 * descriptors now
+	 */
+	const int is_hs = func->function.hs_descriptors != NULL;
+	const int is_ss = func->function.ss_descriptors != NULL;
 	unsigned ep_desc_id, idx;
-	static const char *speed_names[] = { "full", "high", "super" };
 
 	if (type != FFS_DESCRIPTOR)
 		return 0;
 
-	/*
-	 * If ss_descriptors is not NULL, we are reading super speed
-	 * descriptors; if hs_descriptors is not NULL, we are reading high
-	 * speed descriptors; otherwise, we are reading full speed
-	 * descriptors.
-	 */
-	if (func->function.ss_descriptors) {
-		ep_desc_id = 2;
+	if (is_ss) {
 		func->function.ss_descriptors[(long)valuep] = desc;
-	} else if (func->function.hs_descriptors) {
-		ep_desc_id = 1;
+		ep_desc_id = 2;
+	} else if (is_hs) {
 		func->function.hs_descriptors[(long)valuep] = desc;
+		ep_desc_id = 1;
 	} else {
-		ep_desc_id = 0;
 		func->function.fs_descriptors[(long)valuep]    = desc;
+		ep_desc_id = 0;
 	}
 
 	if (!desc || desc->bDescriptorType != USB_DT_ENDPOINT)
@@ -2157,8 +2229,8 @@ static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
 	ffs_ep = func->eps + idx;
 
 	if (unlikely(ffs_ep->descs[ep_desc_id])) {
-		pr_err("two %sspeed descriptors for EP %d\n",
-			  speed_names[ep_desc_id],
+		pr_vdebug("two %sspeed descriptors for EP %d\n",
+			  is_ss ? "super" : "high/full",
 			  ds->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
 		return -EINVAL;
 	}
@@ -2277,7 +2349,7 @@ static int ffs_func_bind(struct usb_configuration *c,
 
 	ENTER();
 
-	/* Has descriptors only for speeds gadget does not support */
+	/* Only high/super speed but not supported by gadget? */
 	if (unlikely(!(full | high | super)))
 		return -ENOTSUPP;
 
@@ -2288,9 +2360,14 @@ static int ffs_func_bind(struct usb_configuration *c,
 
 	/* Zero */
 	memset(data->eps, 0, sizeof data->eps);
-	/* Copy descriptors */
-	memcpy(data->raw_descs, ffs->raw_descs,
-	       ffs->raw_descs_length);
+	/* Copy only raw (hs,fs) descriptors (until ss_magic and ss_count) */
+	memcpy(data->raw_descs, ffs->raw_descs + 16,
+				ffs->raw_fs_hs_descs_length);
+	/* Copy SS descriptors */
+	if (func->ffs->ss_descs_count)
+		memcpy(data->raw_descs + ffs->raw_fs_hs_descs_length,
+			ffs->raw_descs + ffs->raw_ss_descs_offset,
+			ffs->raw_ss_descs_length);
 
 	memset(data->inums, 0xff, sizeof data->inums);
 	for (ret = ffs->eps_count; ret; --ret)
@@ -2308,9 +2385,9 @@ static int ffs_func_bind(struct usb_configuration *c,
 	if (likely(full)) {
 		func->function.fs_descriptors = data->fs_descs;
 		fs_len = ffs_do_descs(ffs->fs_descs_count,
-				      data->raw_descs,
-				      sizeof data->raw_descs,
-				      __ffs_func_bind_do_descs, func);
+				   data->raw_descs,
+				   sizeof(data->raw_descs),
+				   __ffs_func_bind_do_descs, func);
 		if (unlikely(fs_len < 0)) {
 			ret = fs_len;
 			goto error;
@@ -2322,9 +2399,9 @@ static int ffs_func_bind(struct usb_configuration *c,
 	if (likely(high)) {
 		func->function.hs_descriptors = data->hs_descs;
 		hs_len = ffs_do_descs(ffs->hs_descs_count,
-				      data->raw_descs + fs_len,
-				      (sizeof data->raw_descs) - fs_len,
-				      __ffs_func_bind_do_descs, func);
+				   data->raw_descs + fs_len,
+				   (sizeof(data->raw_descs)) - fs_len,
+				   __ffs_func_bind_do_descs, func);
 		if (unlikely(hs_len < 0)) {
 			ret = hs_len;
 			goto error;
@@ -2336,9 +2413,9 @@ static int ffs_func_bind(struct usb_configuration *c,
 	if (likely(super)) {
 		func->function.ss_descriptors = data->ss_descs;
 		ret = ffs_do_descs(ffs->ss_descs_count,
-				data->raw_descs + fs_len + hs_len,
-				(sizeof data->raw_descs) - fs_len - hs_len,
-				__ffs_func_bind_do_descs, func);
+				   data->raw_descs + fs_len + hs_len,
+				   (sizeof(data->raw_descs)) - fs_len - hs_len,
+				   __ffs_func_bind_do_descs, func);
 		if (unlikely(ret < 0))
 			goto error;
 	}
@@ -2351,7 +2428,7 @@ static int ffs_func_bind(struct usb_configuration *c,
 	ret = ffs_do_descs(ffs->fs_descs_count +
 			   (high ? ffs->hs_descs_count : 0) +
 			   (super ? ffs->ss_descs_count : 0),
-			   data->raw_descs, sizeof data->raw_descs,
+			   data->raw_descs, sizeof(data->raw_descs),
 			   __ffs_func_bind_do_nums, func);
 	if (unlikely(ret < 0))
 		goto error;
@@ -2399,8 +2476,10 @@ static int ffs_func_set_alt(struct usb_function *f,
 			return intf;
 	}
 
-	if (ffs->func)
+	if (ffs->func) {
 		ffs_func_eps_disable(ffs->func);
+		ffs->func = NULL;
+	}
 
 	if (ffs->state != FFS_ACTIVE)
 		return -ENODEV;
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index 97e50b39d68..4dbd9b38504 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -3054,6 +3054,7 @@ static int fsg_main_thread(void *common_)
 static DEVICE_ATTR(ro, 0644, fsg_show_ro, fsg_store_ro);
 static DEVICE_ATTR(nofua, 0644, fsg_show_nofua, fsg_store_nofua);
 static DEVICE_ATTR(file, 0644, fsg_show_file, fsg_store_file);
+static DEVICE_ATTR(cdrom, 0644, fsg_show_cdrom, fsg_store_cdrom);
 
 static struct device_attribute dev_attr_ro_cdrom =
 	__ATTR(ro, 0444, fsg_show_ro, NULL);
@@ -3143,6 +3144,10 @@ static int create_lun_device(struct fsg_common *common,
 		if (rc)
 			goto error_luns;
 
+		rc = device_create_file(&curlun->dev, &dev_attr_cdrom);
+		if (rc)
+			goto error_luns;
+
 #ifdef CONFIG_USB_MSC_PROFILING
 		rc = device_create_file(&curlun->dev, &dev_attr_perf);
 		if (rc)
@@ -3349,6 +3354,7 @@ static void fsg_common_release(struct kref *ref)
 
 		/* In error recovery common->nluns may be zero. */
 		for (; i; --i, ++lun) {
+			device_remove_file(&lun->dev, &dev_attr_cdrom);
 			device_remove_file(&lun->dev, &dev_attr_nofua);
 			device_remove_file(&lun->dev,
 					   lun->cdrom
diff --git a/drivers/usb/gadget/f_mtp.c b/drivers/usb/gadget/f_mtp.c
index 5c41eaf3901..4aefd47387f 100644
--- a/drivers/usb/gadget/f_mtp.c
+++ b/drivers/usb/gadget/f_mtp.c
@@ -38,6 +38,7 @@
 
 #define MTP_BULK_BUFFER_SIZE       131072
 #define INTR_BUFFER_SIZE           28
+#define MTP_MAX_FILE_SIZE          0xFFFFFFFFL
 
 /* String IDs */
 #define INTERFACE_STRING_INDEX	0
@@ -310,7 +311,7 @@ struct {
 		.dwLength = __constant_cpu_to_le32(sizeof(mtp_ext_config_desc)),
 		.bcdVersion = __constant_cpu_to_le16(0x0100),
 		.wIndex = __constant_cpu_to_le16(4),
-		.bCount = __constant_cpu_to_le16(1),
+		.bCount = 1,
 	},
 	.function = {
 		.bFirstInterfaceNumber = 0,
@@ -535,11 +536,12 @@ static ssize_t mtp_read(struct file *fp, char __user *buf,
 	ssize_t r = count;
 	unsigned xfer;
 	int ret = 0;
+	size_t len = 0;
 
 	DBG(cdev, "mtp_read(%zu)\n", count);
 
-	if (count > MTP_BULK_BUFFER_SIZE)
-		return -EINVAL;
+	if (dev == NULL || dev->ep_out == NULL)
+		return -ENODEV;
 
 	/* we will block until we're online */
 	DBG(cdev, "mtp_read: waiting for online state\n");
@@ -550,6 +552,14 @@ static ssize_t mtp_read(struct file *fp, char __user *buf,
 		goto done;
 	}
 	spin_lock_irq(&dev->lock);
+	if (dev->ep_out->desc) {
+		len = usb_ep_align_maybe(cdev->gadget, dev->ep_out, count);
+		if (len > MTP_BULK_BUFFER_SIZE) {
+			spin_unlock_irq(&dev->lock);
+			return -EINVAL;
+		}
+	}
+
 	if (dev->state == STATE_CANCELED) {
 		/* report cancelation to userspace */
 		dev->state = STATE_READY;
@@ -562,7 +572,7 @@ static ssize_t mtp_read(struct file *fp, char __user *buf,
 requeue_req:
 	/* queue a request */
 	req = dev->rx_req[0];
-	req->length = count;
+	req->length = len;
 	dev->rx_done = 0;
 	set_read_req_length(req);
 	ret = usb_ep_queue(dev->ep_out, req, GFP_KERNEL);
@@ -717,10 +727,6 @@ static void send_file_work(struct work_struct *data)
 	filp = dev->xfer_file;
 	offset = dev->xfer_file_offset;
 	count = dev->xfer_file_length;
-	if (count < 0) {
-		dev->xfer_result = -EINVAL;
-		return;
-	}
 
 	DBG(cdev, "send_file_work(%lld %lld)\n", offset, count);
 
@@ -764,7 +770,12 @@ static void send_file_work(struct work_struct *data)
 		if (hdr_size) {
 			/* prepend MTP data header */
 			header = (struct mtp_data_header *)req->buf;
-			header->length = __cpu_to_le32(count);
+			/*
+			 * set file size with header according to
+			 * MTP Specification v1.0
+			 */
+			header->length = (count > MTP_MAX_FILE_SIZE) ?
+				MTP_MAX_FILE_SIZE : __cpu_to_le32(count);
 			header->type = __cpu_to_le16(2); /* data packet */
 			header->command = __cpu_to_le16(dev->xfer_command);
 			header->transaction_id =
@@ -822,10 +833,6 @@ static void receive_file_work(struct work_struct *data)
 	filp = dev->xfer_file;
 	offset = dev->xfer_file_offset;
 	count = dev->xfer_file_length;
-	if (count < 0) {
-		dev->xfer_result = -EINVAL;
-		return;
-	}
 
 	DBG(cdev, "receive_file_work(%lld)\n", count);
 
@@ -871,6 +878,10 @@ static void receive_file_work(struct work_struct *data)
 					usb_ep_dequeue(dev->ep_out, read_req);
 				break;
 			}
+			if (read_req->status) {
+				r = read_req->status;
+				break;
+			}
 			/* if xfer_file_length is 0xFFFFFFFF, then we read until
 			 * we get a zero length packet
 			 */
@@ -1288,6 +1299,21 @@ static int mtp_bind_config(struct usb_configuration *c, bool ptp_config)
 
 	printk(KERN_INFO "mtp_bind_config\n");
 
+	/*
+	 * PTP piggybacks on MTP function so make sure we have
+	 * created MTP function before we associate this PTP
+	 * function with a gadget configuration.
+	 */
+	if (dev == NULL) {
+		pr_err("Error: Create MTP function before linking"
+				" PTP function with a gadget configuration\n");
+		pr_err("\t1: Delete existing PTP function if any\n");
+		pr_err("\t2: Create MTP function\n");
+		pr_err("\t3: Create and symlink PTP function"
+				" with a gadget configuration\n");
+		return -EINVAL; /* Invalid Configuration */
+	}
+
 	/* allocate a string ID for our interface */
 	if (mtp_string_defs[INTERFACE_STRING_INDEX].id == 0) {
 		ret = usb_string_id(c->cdev);
diff --git a/drivers/usb/gadget/f_mtp_samsung.c b/drivers/usb/gadget/f_mtp_samsung.c
index 55ce9eadff5..84bfb948922 100644
--- a/drivers/usb/gadget/f_mtp_samsung.c
+++ b/drivers/usb/gadget/f_mtp_samsung.c
@@ -1157,7 +1157,10 @@ static long  mtpg_ioctl(struct file *fd, unsigned int code, unsigned long arg)
 		max_pkt = dev->bulk_in->maxpacket;
 		printk(KERN_DEBUG "[%s] line = %d max_pkt = [%d]\n",
 						 __func__, __LINE__, max_pkt);
-		status = max_pkt;
+		if (max_pkt == 64)
+			status = 64;
+		else
+			status = 512;
 		break;
 	case SEND_FILE_WITH_HEADER:
 	{
diff --git a/drivers/usb/gadget/functions.c b/drivers/usb/gadget/functions.c
index b13f839e736..389c1f3d0fe 100644
--- a/drivers/usb/gadget/functions.c
+++ b/drivers/usb/gadget/functions.c
@@ -58,7 +58,7 @@ struct usb_function *usb_get_function(struct usb_function_instance *fi)
 	struct usb_function *f;
 
 	f = fi->fd->alloc_func(fi);
-	if (IS_ERR(f))
+	if ((f == NULL) || IS_ERR(f))
 		return f;
 	f->fi = fi;
 	return f;
diff --git a/drivers/usb/gadget/multi_config.c b/drivers/usb/gadget/multi_config.c
index 2d7681aa663..a42df7b6e1d 100644
--- a/drivers/usb/gadget/multi_config.c
+++ b/drivers/usb/gadget/multi_config.c
@@ -98,9 +98,6 @@ unsigned count_multi_config(struct usb_configuration *c, unsigned count)
 		} else if (!strcmp(f->name, MULTI_EXCEPTION_FUNCTION)) {
 			USB_DBG("exception %s +\n", MULTI_EXCEPTION_FUNCTION);
 			f_exception = 1;
-		} else if (!strcmp(f->name, MULTI_EXCEPTION_FUNCTION) || !strcmp(f->name, MULTI_EXCEPTION_FUNCTION_F_FS)) {
-			USB_DBG("exception %s +\n", f->name);
-			f_exception = 1;
 		}
 	}
 
diff --git a/drivers/usb/gadget/multi_config.h b/drivers/usb/gadget/multi_config.h
index 447f97b344b..5ea2b349119 100644
--- a/drivers/usb/gadget/multi_config.h
+++ b/drivers/usb/gadget/multi_config.h
@@ -61,7 +61,6 @@
 #define MULTI_FUNCTION_1 "mtp"
 #define MULTI_FUNCTION_2 "acm0"
 #define MULTI_EXCEPTION_FUNCTION "adb"
-#define MULTI_EXCEPTION_FUNCTION_F_FS "Function FS Gadget"
 
 #define MAC_REQUEST	0
 #define OTHER_REQUEST	1
diff --git a/drivers/usb/gadget/storage_common.c b/drivers/usb/gadget/storage_common.c
index 94766dfa508..096c6fa2991 100644
--- a/drivers/usb/gadget/storage_common.c
+++ b/drivers/usb/gadget/storage_common.c
@@ -568,6 +568,14 @@ static ssize_t fsg_show_nofua(struct device *dev, struct device_attribute *attr,
 	return sprintf(buf, "%u\n", curlun->nofua);
 }
 
+static ssize_t fsg_show_cdrom (struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct fsg_lun  *curlun = fsg_lun_from_dev(dev);
+
+	return sprintf(buf, "%d\n", curlun->cdrom);
+}
+
 static ssize_t fsg_show_file(struct device *dev, struct device_attribute *attr,
 			     char *buf)
 {
@@ -681,3 +689,32 @@ static ssize_t fsg_store_file(struct device *dev, struct device_attribute *attr,
 	up_write(filesem);
 	return (rc < 0 ? rc : count);
 }
+
+static ssize_t fsg_store_cdrom(struct device *dev, struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	ssize_t    rc;
+	struct fsg_lun  *curlun = fsg_lun_from_dev(dev);
+	struct rw_semaphore  *filesem = dev_get_drvdata(dev);
+	unsigned  cdrom;
+
+	rc = kstrtouint(buf, 2, &cdrom);
+	if (rc)
+		return rc;
+
+	/*
+	 * Allow the cdrom status to change only while the
+	 * backing file is closed.
+	 */
+	down_read(filesem);
+	if (fsg_lun_is_open(curlun)) {
+		LDBG(curlun, "cdrom status change prevented\n");
+		rc = -EBUSY;
+	} else {
+		curlun->cdrom = cdrom;
+		LDBG(curlun, "cdrom status set to %d\n", curlun->cdrom);
+		rc = count;
+	}
+	up_read(filesem);
+	return rc;
+} 
diff --git a/include/uapi/linux/usb/functionfs.h b/include/uapi/linux/usb/functionfs.h
index 2592d867499..0f8f7be5b0d 100644
--- a/include/uapi/linux/usb/functionfs.h
+++ b/include/uapi/linux/usb/functionfs.h
@@ -10,15 +10,10 @@
 
 enum {
 	FUNCTIONFS_DESCRIPTORS_MAGIC = 1,
-	FUNCTIONFS_STRINGS_MAGIC = 2,
-	FUNCTIONFS_DESCRIPTORS_MAGIC_V2 = 3,
+	FUNCTIONFS_STRINGS_MAGIC     = 2
 };
 
-enum functionfs_flags {
-	FUNCTIONFS_HAS_FS_DESC = 1,
-	FUNCTIONFS_HAS_HS_DESC = 2,
-	FUNCTIONFS_HAS_SS_DESC = 4,
-};
+#define FUNCTIONFS_SS_DESC_MAGIC 0x0055DE5C
 
 #ifndef __KERNEL__
 
@@ -33,58 +28,35 @@ struct usb_endpoint_descriptor_no_audio {
 	__u8  bInterval;
 } __attribute__((packed));
 
-struct usb_functionfs_descs_head_v2 {
-	__le32 magic;
-	__le32 length;
-	__le32 flags;
-	/*
-	 * __le32 fs_count, hs_count, fs_count; must be included manually in
-	 * the structure taking flags into consideration.
-	 */
-} __attribute__((packed));
 
-/* Legacy format, deprecated as of 3.14. */
+/*
+ * All numbers must be in little endian order.
+ */
+
 struct usb_functionfs_descs_head {
 	__le32 magic;
 	__le32 length;
 	__le32 fs_count;
 	__le32 hs_count;
-} __attribute__((packed, deprecated));
+} __attribute__((packed));
 
 /*
  * Descriptors format:
  *
  * | off | name      | type         | description                          |
  * |-----+-----------+--------------+--------------------------------------|
- * |   0 | magic     | LE32         | FUNCTIONFS_DESCRIPTORS_MAGIC_V2      |
- * |   4 | length    | LE32         | length of the whole data chunk       |
- * |   8 | flags     | LE32         | combination of functionfs_flags      |
- * |     | fs_count  | LE32         | number of full-speed descriptors     |
- * |     | hs_count  | LE32         | number of high-speed descriptors     |
- * |     | ss_count  | LE32         | number of super-speed descriptors    |
- * |     | fs_descrs | Descriptor[] | list of full-speed descriptors       |
- * |     | hs_descrs | Descriptor[] | list of high-speed descriptors       |
- * |     | ss_descrs | Descriptor[] | list of super-speed descriptors      |
- *
- * Depending on which flags are set, various fields may be missing in the
- * structure.  Any flags that are not recognised cause the whole block to be
- * rejected with -ENOSYS.
- *
- * Legacy descriptors format:
- *
- * | off | name      | type         | description                          |
- * |-----+-----------+--------------+--------------------------------------|
- * |   0 | magic     | LE32         | FUNCTIONFS_DESCRIPTORS_MAGIC         |
+ * |   0 | magic     | LE32         | FUNCTIONFS_{FS,HS}_DESCRIPTORS_MAGIC |
  * |   4 | length    | LE32         | length of the whole data chunk       |
  * |   8 | fs_count  | LE32         | number of full-speed descriptors     |
  * |  12 | hs_count  | LE32         | number of high-speed descriptors     |
  * |  16 | fs_descrs | Descriptor[] | list of full-speed descriptors       |
  * |     | hs_descrs | Descriptor[] | list of high-speed descriptors       |
+ * |     | ss_magic  | LE32         | FUNCTIONFS_SS_DESC_MAGIC             |
+ * |     | ss_count  | LE32         | number of super-speed descriptors    |
+ * |     | ss_descrs | Descriptor[] | list of super-speed descriptors      |
  *
- * All numbers must be in little endian order.
- *
- * Descriptor[] is an array of valid USB descriptors which have the following
- * format:
+ * ss_magic: if present then it implies that SS_DESCs are also present
+ * descs are just valid USB descriptors and have the following format:
  *
  * | off | name            | type | description              |
  * |-----+-----------------+------+--------------------------|