Merge tag 'usb-6.3-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/usb

Pull USB / Thunderbolt driver updates from Greg KH:
 "Here is the big set of USB and Thunderbolt driver changes for 6.3-rc1.

  Nothing major in here, just lots of good development, including:

   - Thunderbolt additions for new device support and features

   - xhci driver updates and cleanups

   - USB gadget media driver updates (includes media core changes that
     were acked by the v4l2 maintainers)

   - lots of other USB gadget driver updates for new features

   - dwc3 driver updates and fixes

   - minor debugfs leak fixes

   - typec driver updates and additions

   - dt-bindings conversions to yaml

   - other small bugfixes and driver updates

  All have been in linux-next for a while with no reported issues"

* tag 'usb-6.3-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/usb: (237 commits)
  usb: dwc3: xilinx: Remove unused of_gpio,h
  usb: typec: pd: Add higher capability sysfs for sink PDO
  usb: typec: pd: Remove usb_suspend_supported sysfs from sink PDO
  usb: dwc3: pci: add support for the Intel Meteor Lake-M
  usb: gadget: u_ether: Don't warn in gether_setup_name_default()
  usb: gadget: u_ether: Convert prints to device prints
  usb: gadget: u_serial: Add null pointer check in gserial_resume
  usb: gadget: uvc: fix missing mutex_unlock() if kstrtou8() fails
  xhci: host: potential NULL dereference in xhci_generic_plat_probe()
  dt-bindings: usb: amlogic,meson-g12a-usb-ctrl: make G12A usb3-phy0 optional
  usb: host: fsl-mph-dr-of: reuse device_set_of_node_from_dev
  of: device: Do not ignore error code in of_device_uevent_modalias
  of: device: Ignore modalias of reused nodes
  usb: gadget: configfs: Fix set but not used variable warning
  usb: gadget: uvc: Use custom strings if available
  usb: gadget: uvc: Allow linking function to string descs
  usb: gadget: uvc: Pick up custom string descriptor IDs
  usb: gadget: uvc: Allow linking XUs to string descriptors
  usb: gadget: configfs: Attach arbitrary strings to cdev
  usb: gadget: configfs: Support arbitrary string descriptors
  ...

25 files changed, 5484 insertions, 327 deletions

diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 4fa2ddf322b4..336db8f92afa 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -203,6 +203,7 @@ config USB_F_UAC2
 
 config USB_F_UVC
 	tristate
+	select UVC_COMMON
 
 config USB_F_MIDI
 	tristate
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index 403563c06477..fa7dd6cf014d 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -14,9 +14,11 @@
 #include <linux/device.h>
 #include <linux/utsname.h>
 #include <linux/bitfield.h>
+#include <linux/uuid.h>
 
 #include <linux/usb/composite.h>
 #include <linux/usb/otg.h>
+#include <linux/usb/webusb.h>
 #include <asm/unaligned.h>
 
 #include "u_os_desc.h"
@@ -713,14 +715,16 @@ static int bos_desc(struct usb_composite_dev *cdev)
 	 * A SuperSpeed device shall include the USB2.0 extension descriptor
 	 * and shall support LPM when operating in USB2.0 HS mode.
 	 */
-	usb_ext = cdev->req->buf + le16_to_cpu(bos->wTotalLength);
-	bos->bNumDeviceCaps++;
-	le16_add_cpu(&bos->wTotalLength, USB_DT_USB_EXT_CAP_SIZE);
-	usb_ext->bLength = USB_DT_USB_EXT_CAP_SIZE;
-	usb_ext->bDescriptorType = USB_DT_DEVICE_CAPABILITY;
-	usb_ext->bDevCapabilityType = USB_CAP_TYPE_EXT;
-	usb_ext->bmAttributes = cpu_to_le32(USB_LPM_SUPPORT |
-					    USB_BESL_SUPPORT | besl);
+	if (cdev->gadget->lpm_capable) {
+		usb_ext = cdev->req->buf + le16_to_cpu(bos->wTotalLength);
+		bos->bNumDeviceCaps++;
+		le16_add_cpu(&bos->wTotalLength, USB_DT_USB_EXT_CAP_SIZE);
+		usb_ext->bLength = USB_DT_USB_EXT_CAP_SIZE;
+		usb_ext->bDescriptorType = USB_DT_DEVICE_CAPABILITY;
+		usb_ext->bDevCapabilityType = USB_CAP_TYPE_EXT;
+		usb_ext->bmAttributes = cpu_to_le32(USB_LPM_SUPPORT |
+							USB_BESL_SUPPORT | besl);
+	}
 
 	/*
 	 * The Superspeed USB Capability descriptor shall be implemented by all
@@ -821,6 +825,37 @@ static int bos_desc(struct usb_composite_dev *cdev)
 		}
 	}
 
+	/* The WebUSB Platform Capability descriptor */
+	if (cdev->use_webusb) {
+		struct usb_plat_dev_cap_descriptor *webusb_cap;
+		struct usb_webusb_cap_data *webusb_cap_data;
+		guid_t webusb_uuid = WEBUSB_UUID;
+
+		webusb_cap = cdev->req->buf + le16_to_cpu(bos->wTotalLength);
+		webusb_cap_data = (struct usb_webusb_cap_data *) webusb_cap->CapabilityData;
+		bos->bNumDeviceCaps++;
+		le16_add_cpu(&bos->wTotalLength,
+			USB_DT_USB_PLAT_DEV_CAP_SIZE(USB_WEBUSB_CAP_DATA_SIZE));
+
+		webusb_cap->bLength = USB_DT_USB_PLAT_DEV_CAP_SIZE(USB_WEBUSB_CAP_DATA_SIZE);
+		webusb_cap->bDescriptorType = USB_DT_DEVICE_CAPABILITY;
+		webusb_cap->bDevCapabilityType = USB_PLAT_DEV_CAP_TYPE;
+		webusb_cap->bReserved = 0;
+		export_guid(webusb_cap->UUID, &webusb_uuid);
+
+		if (cdev->bcd_webusb_version != 0)
+			webusb_cap_data->bcdVersion = cpu_to_le16(cdev->bcd_webusb_version);
+		else
+			webusb_cap_data->bcdVersion = WEBUSB_VERSION_1_00;
+
+		webusb_cap_data->bVendorCode = cdev->b_webusb_vendor_code;
+
+		if (strnlen(cdev->landing_page, sizeof(cdev->landing_page)) > 0)
+			webusb_cap_data->iLandingPage = WEBUSB_LANDING_PAGE_PRESENT;
+		else
+			webusb_cap_data->iLandingPage = WEBUSB_LANDING_PAGE_NOT_PRESENT;
+	}
+
 	return le16_to_cpu(bos->wTotalLength);
 }
 
@@ -1744,7 +1779,7 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 					cdev->desc.bcdUSB = cpu_to_le16(0x0210);
 				}
 			} else {
-				if (gadget->lpm_capable)
+				if (gadget->lpm_capable || cdev->use_webusb)
 					cdev->desc.bcdUSB = cpu_to_le16(0x0201);
 				else
 					cdev->desc.bcdUSB = cpu_to_le16(0x0200);
@@ -1779,7 +1814,7 @@ composite_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
 			break;
 		case USB_DT_BOS:
 			if (gadget_is_superspeed(gadget) ||
-			    gadget->lpm_capable) {
+			    gadget->lpm_capable || cdev->use_webusb) {
 				value = bos_desc(cdev);
 				value = min(w_length, (u16) value);
 			}
@@ -2013,6 +2048,53 @@ unknown:
 			goto check_value;
 		}
 
+		/*
+		 * WebUSB URL descriptor handling, following:
+		 * https://wicg.github.io/webusb/#device-requests
+		 */
+		if (cdev->use_webusb &&
+		    ctrl->bRequestType == (USB_DIR_IN | USB_TYPE_VENDOR) &&
+		    w_index == WEBUSB_GET_URL &&
+		    w_value == WEBUSB_LANDING_PAGE_PRESENT &&
+		    ctrl->bRequest == cdev->b_webusb_vendor_code) {
+			unsigned int	landing_page_length;
+			unsigned int	landing_page_offset;
+			struct webusb_url_descriptor *url_descriptor =
+					(struct webusb_url_descriptor *)cdev->req->buf;
+
+			url_descriptor->bDescriptorType = WEBUSB_URL_DESCRIPTOR_TYPE;
+
+			if (strncasecmp(cdev->landing_page, "https://",  8) == 0) {
+				landing_page_offset = 8;
+				url_descriptor->bScheme = WEBUSB_URL_SCHEME_HTTPS;
+			} else if (strncasecmp(cdev->landing_page, "http://", 7) == 0) {
+				landing_page_offset = 7;
+				url_descriptor->bScheme = WEBUSB_URL_SCHEME_HTTP;
+			} else {
+				landing_page_offset = 0;
+				url_descriptor->bScheme = WEBUSB_URL_SCHEME_NONE;
+			}
+
+			landing_page_length = strnlen(cdev->landing_page,
+				sizeof(url_descriptor->URL)
+				- WEBUSB_URL_DESCRIPTOR_HEADER_LENGTH + landing_page_offset);
+
+			if (ctrl->wLength < WEBUSB_URL_DESCRIPTOR_HEADER_LENGTH
+					    + landing_page_length)
+				landing_page_length = ctrl->wLength
+					- WEBUSB_URL_DESCRIPTOR_HEADER_LENGTH + landing_page_offset;
+
+			memcpy(url_descriptor->URL,
+				cdev->landing_page + landing_page_offset,
+				landing_page_length - landing_page_offset);
+			url_descriptor->bLength = landing_page_length
+				- landing_page_offset + WEBUSB_URL_DESCRIPTOR_HEADER_LENGTH;
+
+			value = url_descriptor->bLength;
+
+			goto check_value;
+		}
+
 		VDBG(cdev,
 			"non-core control req%02x.%02x v%04x i%04x l%d\n",
 			ctrl->bRequestType, ctrl->bRequest,
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index 0853536cbf2e..b9f1136aa0a2 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -7,6 +7,7 @@
 #include <linux/nls.h>
 #include <linux/usb/composite.h>
 #include <linux/usb/gadget_configfs.h>
+#include <linux/usb/webusb.h>
 #include "configfs.h"
 #include "u_f.h"
 #include "u_os_desc.h"
@@ -39,6 +40,7 @@ struct gadget_info {
 	struct config_group configs_group;
 	struct config_group strings_group;
 	struct config_group os_desc_group;
+	struct config_group webusb_group;
 
 	struct mutex lock;
 	struct usb_gadget_strings *gstrings[MAX_USB_STRING_LANGS + 1];
@@ -50,6 +52,11 @@ struct gadget_info {
 	bool use_os_desc;
 	char b_vendor_code;
 	char qw_sign[OS_STRING_QW_SIGN_LEN];
+	bool use_webusb;
+	u16 bcd_webusb_version;
+	u8 b_webusb_vendor_code;
+	char landing_page[WEBUSB_URL_RAW_MAX_LENGTH];
+
 	spinlock_t spinlock;
 	bool unbind;
 };
@@ -79,7 +86,7 @@ static inline struct gadget_info *cfg_to_gadget_info(struct config_usb_cfg *cfg)
 	return container_of(cfg->c.cdev, struct gadget_info, cdev);
 }
 
-struct gadget_strings {
+struct gadget_language {
 	struct usb_gadget_strings stringtab_dev;
 	struct usb_string strings[USB_GADGET_FIRST_AVAIL_IDX];
 	char *manufacturer;
@@ -88,6 +95,8 @@ struct gadget_strings {
 
 	struct config_group group;
 	struct list_head list;
+	struct list_head gadget_strings;
+	unsigned int nstrings;
 };
 
 struct gadget_config_name {
@@ -365,9 +374,9 @@ static struct configfs_attribute *gadget_root_attrs[] = {
 	NULL,
 };
 
-static inline struct gadget_strings *to_gadget_strings(struct config_item *item)
+static inline struct gadget_language *to_gadget_language(struct config_item *item)
 {
-	return container_of(to_config_group(item), struct gadget_strings,
+	return container_of(to_config_group(item), struct gadget_language,
 			 group);
 }
 
@@ -430,6 +439,12 @@ static int config_usb_cfg_link(
 	 * from another gadget or a random directory.
 	 * Also a function instance can only be linked once.
 	 */
+
+	if (gi->composite.gadget_driver.udc_name) {
+		ret = -EINVAL;
+		goto out;
+	}
+
 	list_for_each_entry(iter, &gi->available_func, cfs_list) {
 		if (iter != fi)
 			continue;
@@ -755,20 +770,20 @@ static const struct config_item_type config_desc_type = {
 	.ct_owner       = THIS_MODULE,
 };
 
-GS_STRINGS_RW(gadget_strings, manufacturer);
-GS_STRINGS_RW(gadget_strings, product);
-GS_STRINGS_RW(gadget_strings, serialnumber);
+GS_STRINGS_RW(gadget_language, manufacturer);
+GS_STRINGS_RW(gadget_language, product);
+GS_STRINGS_RW(gadget_language, serialnumber);
 
-static struct configfs_attribute *gadget_strings_langid_attrs[] = {
-	&gadget_strings_attr_manufacturer,
-	&gadget_strings_attr_product,
-	&gadget_strings_attr_serialnumber,
+static struct configfs_attribute *gadget_language_langid_attrs[] = {
+	&gadget_language_attr_manufacturer,
+	&gadget_language_attr_product,
+	&gadget_language_attr_serialnumber,
 	NULL,
 };
 
-static void gadget_strings_attr_release(struct config_item *item)
+static void gadget_language_attr_release(struct config_item *item)
 {
-	struct gadget_strings *gs = to_gadget_strings(item);
+	struct gadget_language *gs = to_gadget_language(item);
 
 	kfree(gs->manufacturer);
 	kfree(gs->product);
@@ -778,8 +793,317 @@ static void gadget_strings_attr_release(struct config_item *item)
 	kfree(gs);
 }
 
-USB_CONFIG_STRING_RW_OPS(gadget_strings);
-USB_CONFIG_STRINGS_LANG(gadget_strings, gadget_info);
+static struct configfs_item_operations gadget_language_langid_item_ops = {
+	.release                = gadget_language_attr_release,
+};
+
+static ssize_t gadget_string_id_show(struct config_item *item, char *page)
+{
+	struct gadget_string *string = to_gadget_string(item);
+	int ret;
+
+	ret = sprintf(page, "%u\n", string->usb_string.id);
+	return ret;
+}
+CONFIGFS_ATTR_RO(gadget_string_, id);
+
+static ssize_t gadget_string_s_show(struct config_item *item, char *page)
+{
+	struct gadget_string *string = to_gadget_string(item);
+	int ret;
+
+	ret = snprintf(page, sizeof(string->string), "%s\n", string->string);
+	return ret;
+}
+
+static ssize_t gadget_string_s_store(struct config_item *item, const char *page,
+				     size_t len)
+{
+	struct gadget_string *string = to_gadget_string(item);
+	int size = min(sizeof(string->string), len + 1);
+
+	if (len > USB_MAX_STRING_LEN)
+		return -EINVAL;
+
+	return strscpy(string->string, page, size);
+}
+CONFIGFS_ATTR(gadget_string_, s);
+
+static struct configfs_attribute *gadget_string_attrs[] = {
+	&gadget_string_attr_id,
+	&gadget_string_attr_s,
+	NULL,
+};
+
+static void gadget_string_release(struct config_item *item)
+{
+	struct gadget_string *string = to_gadget_string(item);
+
+	kfree(string);
+}
+
+static struct configfs_item_operations gadget_string_item_ops = {
+	.release	= gadget_string_release,
+};
+
+static const struct config_item_type gadget_string_type = {
+	.ct_item_ops	= &gadget_string_item_ops,
+	.ct_attrs	= gadget_string_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static struct config_item *gadget_language_string_make(struct config_group *group,
+						       const char *name)
+{
+	struct gadget_language *language;
+	struct gadget_string *string;
+
+	language = to_gadget_language(&group->cg_item);
+
+	string = kzalloc(sizeof(*string), GFP_KERNEL);
+	if (!string)
+		return ERR_PTR(-ENOMEM);
+
+	string->usb_string.id = language->nstrings++;
+	string->usb_string.s = string->string;
+	list_add_tail(&string->list, &language->gadget_strings);
+
+	config_item_init_type_name(&string->item, name, &gadget_string_type);
+
+	return &string->item;
+}
+
+static void gadget_language_string_drop(struct config_group *group,
+					struct config_item *item)
+{
+	struct gadget_language *language;
+	struct gadget_string *string;
+	unsigned int i = USB_GADGET_FIRST_AVAIL_IDX;
+
+	language = to_gadget_language(&group->cg_item);
+	string = to_gadget_string(item);
+
+	list_del(&string->list);
+	language->nstrings--;
+
+	/* Reset the ids for the language's strings to guarantee a continuous set */
+	list_for_each_entry(string, &language->gadget_strings, list)
+		string->usb_string.id = i++;
+}
+
+static struct configfs_group_operations gadget_language_langid_group_ops = {
+	.make_item		= gadget_language_string_make,
+	.drop_item		= gadget_language_string_drop,
+};
+
+static struct config_item_type gadget_language_type = {
+	.ct_item_ops	= &gadget_language_langid_item_ops,
+	.ct_group_ops	= &gadget_language_langid_group_ops,
+	.ct_attrs	= gadget_language_langid_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static struct config_group *gadget_language_make(struct config_group *group,
+						 const char *name)
+{
+	struct gadget_info *gi;
+	struct gadget_language *gs;
+	struct gadget_language *new;
+	int langs = 0;
+	int ret;
+
+	new = kzalloc(sizeof(*new), GFP_KERNEL);
+	if (!new)
+		return ERR_PTR(-ENOMEM);
+
+	ret = check_user_usb_string(name, &new->stringtab_dev);
+	if (ret)
+		goto err;
+	config_group_init_type_name(&new->group, name,
+				    &gadget_language_type);
+
+	gi = container_of(group, struct gadget_info, strings_group);
+	ret = -EEXIST;
+	list_for_each_entry(gs, &gi->string_list, list) {
+		if (gs->stringtab_dev.language == new->stringtab_dev.language)
+			goto err;
+		langs++;
+	}
+	ret = -EOVERFLOW;
+	if (langs >= MAX_USB_STRING_LANGS)
+		goto err;
+
+	list_add_tail(&new->list, &gi->string_list);
+	INIT_LIST_HEAD(&new->gadget_strings);
+
+	/* We have the default manufacturer, product and serialnumber strings */
+	new->nstrings = 3;
+	return &new->group;
+err:
+	kfree(new);
+	return ERR_PTR(ret);
+}
+
+static void gadget_language_drop(struct config_group *group,
+				 struct config_item *item)
+{
+	config_item_put(item);
+}
+
+static struct configfs_group_operations gadget_language_group_ops = {
+	.make_group     = &gadget_language_make,
+	.drop_item      = &gadget_language_drop,
+};
+
+static struct config_item_type gadget_language_strings_type = {
+	.ct_group_ops   = &gadget_language_group_ops,
+	.ct_owner       = THIS_MODULE,
+};
+
+static inline struct gadget_info *webusb_item_to_gadget_info(
+		struct config_item *item)
+{
+	return container_of(to_config_group(item),
+			struct gadget_info, webusb_group);
+}
+
+static ssize_t webusb_use_show(struct config_item *item, char *page)
+{
+	return sysfs_emit(page, "%d\n",
+			webusb_item_to_gadget_info(item)->use_webusb);
+}
+
+static ssize_t webusb_use_store(struct config_item *item, const char *page,
+				 size_t len)
+{
+	struct gadget_info *gi = webusb_item_to_gadget_info(item);
+	int ret;
+	bool use;
+
+	ret = kstrtobool(page, &use);
+	if (ret)
+		return ret;
+
+	mutex_lock(&gi->lock);
+	gi->use_webusb = use;
+	mutex_unlock(&gi->lock);
+
+	return len;
+}
+
+static ssize_t webusb_bcdVersion_show(struct config_item *item, char *page)
+{
+	return sysfs_emit(page, "0x%04x\n",
+					webusb_item_to_gadget_info(item)->bcd_webusb_version);
+}
+
+static ssize_t webusb_bcdVersion_store(struct config_item *item,
+		const char *page, size_t len)
+{
+	struct gadget_info *gi = webusb_item_to_gadget_info(item);
+	u16 bcdVersion;
+	int ret;
+
+	ret = kstrtou16(page, 0, &bcdVersion);
+	if (ret)
+		return ret;
+
+	ret = is_valid_bcd(bcdVersion);
+	if (ret)
+		return ret;
+
+	mutex_lock(&gi->lock);
+	gi->bcd_webusb_version = bcdVersion;
+	mutex_unlock(&gi->lock);
+
+	return len;
+}
+
+static ssize_t webusb_bVendorCode_show(struct config_item *item, char *page)
+{
+	return sysfs_emit(page, "0x%02x\n",
+			webusb_item_to_gadget_info(item)->b_webusb_vendor_code);
+}
+
+static ssize_t webusb_bVendorCode_store(struct config_item *item,
+					   const char *page, size_t len)
+{
+	struct gadget_info *gi = webusb_item_to_gadget_info(item);
+	int ret;
+	u8 b_vendor_code;
+
+	ret = kstrtou8(page, 0, &b_vendor_code);
+	if (ret)
+		return ret;
+
+	mutex_lock(&gi->lock);
+	gi->b_webusb_vendor_code = b_vendor_code;
+	mutex_unlock(&gi->lock);
+
+	return len;
+}
+
+static ssize_t webusb_landingPage_show(struct config_item *item, char *page)
+{
+	return sysfs_emit(page, "%s\n", webusb_item_to_gadget_info(item)->landing_page);
+}
+
+static ssize_t webusb_landingPage_store(struct config_item *item, const char *page,
+				     size_t len)
+{
+	struct gadget_info *gi = webusb_item_to_gadget_info(item);
+	unsigned int bytes_to_strip = 0;
+	int l = len;
+
+	if (page[l - 1] == '\n') {
+		--l;
+		++bytes_to_strip;
+	}
+
+	if (l > sizeof(gi->landing_page)) {
+		pr_err("webusb: landingPage URL too long\n");
+		return -EINVAL;
+	}
+
+	// validation
+	if (strncasecmp(page, "https://",  8) == 0)
+		bytes_to_strip = 8;
+	else if (strncasecmp(page, "http://", 7) == 0)
+		bytes_to_strip = 7;
+	else
+		bytes_to_strip = 0;
+
+	if (l > U8_MAX - WEBUSB_URL_DESCRIPTOR_HEADER_LENGTH + bytes_to_strip) {
+		pr_err("webusb: landingPage URL %d bytes too long for given URL scheme\n",
+			l - U8_MAX + WEBUSB_URL_DESCRIPTOR_HEADER_LENGTH - bytes_to_strip);
+		return -EINVAL;
+	}
+
+	mutex_lock(&gi->lock);
+	// ensure 0 bytes are set, in case the new landing page is shorter then the old one.
+	memcpy_and_pad(gi->landing_page, sizeof(gi->landing_page), page, l, 0);
+	mutex_unlock(&gi->lock);
+
+	return len;
+}
+
+CONFIGFS_ATTR(webusb_, use);
+CONFIGFS_ATTR(webusb_, bVendorCode);
+CONFIGFS_ATTR(webusb_, bcdVersion);
+CONFIGFS_ATTR(webusb_, landingPage);
+
+static struct configfs_attribute *webusb_attrs[] = {
+	&webusb_attr_use,
+	&webusb_attr_bcdVersion,
+	&webusb_attr_bVendorCode,
+	&webusb_attr_landingPage,
+	NULL,
+};
+
+static struct config_item_type webusb_type = {
+	.ct_attrs	= webusb_attrs,
+	.ct_owner	= THIS_MODULE,
+};
 
 static inline struct gadget_info *os_desc_item_to_gadget_info(
 		struct config_item *item)
@@ -801,15 +1125,15 @@ static ssize_t os_desc_use_store(struct config_item *item, const char *page,
 	int ret;
 	bool use;
 
-	mutex_lock(&gi->lock);
 	ret = kstrtobool(page, &use);
-	if (!ret) {
-		gi->use_os_desc = use;
-		ret = len;
-	}
+	if (ret)
+		return ret;
+
+	mutex_lock(&gi->lock);
+	gi->use_os_desc = use;
 	mutex_unlock(&gi->lock);
 
-	return ret;
+	return len;
 }
 
 static ssize_t os_desc_b_vendor_code_show(struct config_item *item, char *page)
@@ -825,15 +1149,15 @@ static ssize_t os_desc_b_vendor_code_store(struct config_item *item,
 	int ret;
 	u8 b_vendor_code;
 
-	mutex_lock(&gi->lock);
 	ret = kstrtou8(page, 0, &b_vendor_code);
-	if (!ret) {
-		gi->b_vendor_code = b_vendor_code;
-		ret = len;
-	}
+	if (ret)
+		return ret;
+
+	mutex_lock(&gi->lock);
+	gi->b_vendor_code = b_vendor_code;
 	mutex_unlock(&gi->lock);
 
-	return ret;
+	return len;
 }
 
 static ssize_t os_desc_qw_sign_show(struct config_item *item, char *page)
@@ -958,15 +1282,15 @@ static ssize_t ext_prop_type_store(struct config_item *item,
 	u8 type;
 	int ret;
 
-	if (desc->opts_mutex)
-		mutex_lock(desc->opts_mutex);
 	ret = kstrtou8(page, 0, &type);
 	if (ret)
-		goto end;
-	if (type < USB_EXT_PROP_UNICODE || type > USB_EXT_PROP_UNICODE_MULTI) {
-		ret = -EINVAL;
-		goto end;
-	}
+		return ret;
+
+	if (type < USB_EXT_PROP_UNICODE || type > USB_EXT_PROP_UNICODE_MULTI)
+		return -EINVAL;
+
+	if (desc->opts_mutex)
+		mutex_lock(desc->opts_mutex);
 
 	if ((ext_prop->type == USB_EXT_PROP_BINARY ||
 	    ext_prop->type == USB_EXT_PROP_LE32 ||
@@ -983,12 +1307,10 @@ static ssize_t ext_prop_type_store(struct config_item *item,
 		   type == USB_EXT_PROP_BE32))
 		ext_prop->data_len >>= 1;
 	ext_prop->type = type;
-	ret = len;
 
-end:
 	if (desc->opts_mutex)
 		mutex_unlock(desc->opts_mutex);
-	return ret;
+	return len;
 }
 
 static ssize_t ext_prop_data_show(struct config_item *item, char *page)
@@ -1273,6 +1595,80 @@ static void purge_configs_funcs(struct gadget_info *gi)
 	}
 }
 
+static struct usb_string *
+configfs_attach_gadget_strings(struct gadget_info *gi)
+{
+	struct usb_gadget_strings **gadget_strings;
+	struct gadget_language *language;
+	struct gadget_string *string;
+	unsigned int nlangs = 0;
+	struct list_head *iter;
+	struct usb_string *us;
+	unsigned int i = 0;
+	int nstrings = -1;
+	unsigned int j;
+
+	list_for_each(iter, &gi->string_list)
+		nlangs++;
+
+	/* Bail out early if no languages are configured */
+	if (!nlangs)
+		return NULL;
+
+	gadget_strings = kcalloc(nlangs + 1, /* including NULL terminator */
+				 sizeof(struct usb_gadget_strings *), GFP_KERNEL);
+	if (!gadget_strings)
+		return ERR_PTR(-ENOMEM);
+
+	list_for_each_entry(language, &gi->string_list, list) {
+		struct usb_string *stringtab;
+
+		if (nstrings == -1) {
+			nstrings = language->nstrings;
+		} else if (nstrings != language->nstrings) {
+			pr_err("languages must contain the same number of strings\n");
+			us = ERR_PTR(-EINVAL);
+			goto cleanup;
+		}
+
+		stringtab = kcalloc(language->nstrings + 1, sizeof(struct usb_string),
+				    GFP_KERNEL);
+		if (!stringtab) {
+			us = ERR_PTR(-ENOMEM);
+			goto cleanup;
+		}
+
+		stringtab[USB_GADGET_MANUFACTURER_IDX].id = USB_GADGET_MANUFACTURER_IDX;
+		stringtab[USB_GADGET_MANUFACTURER_IDX].s = language->manufacturer;
+		stringtab[USB_GADGET_PRODUCT_IDX].id = USB_GADGET_PRODUCT_IDX;
+		stringtab[USB_GADGET_PRODUCT_IDX].s = language->product;
+		stringtab[USB_GADGET_SERIAL_IDX].id = USB_GADGET_SERIAL_IDX;
+		stringtab[USB_GADGET_SERIAL_IDX].s = language->serialnumber;
+
+		j = USB_GADGET_FIRST_AVAIL_IDX;
+		list_for_each_entry(string, &language->gadget_strings, list) {
+			memcpy(&stringtab[j], &string->usb_string, sizeof(struct usb_string));
+			j++;
+		}
+
+		language->stringtab_dev.strings = stringtab;
+		gadget_strings[i] = &language->stringtab_dev;
+		i++;
+	}
+
+	us = usb_gstrings_attach(&gi->cdev, gadget_strings, nstrings);
+
+cleanup:
+	list_for_each_entry(language, &gi->string_list, list) {
+		kfree(language->stringtab_dev.strings);
+		language->stringtab_dev.strings = NULL;
+	}
+
+	kfree(gadget_strings);
+
+	return us;
+}
+
 static int configfs_composite_bind(struct usb_gadget *gadget,
 		struct usb_gadget_driver *gdriver)
 {
@@ -1316,22 +1712,7 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 
 	/* init all strings */
 	if (!list_empty(&gi->string_list)) {
-		struct gadget_strings *gs;
-
-		i = 0;
-		list_for_each_entry(gs, &gi->string_list, list) {
-
-			gi->gstrings[i] = &gs->stringtab_dev;
-			gs->stringtab_dev.strings = gs->strings;
-			gs->strings[USB_GADGET_MANUFACTURER_IDX].s =
-				gs->manufacturer;
-			gs->strings[USB_GADGET_PRODUCT_IDX].s = gs->product;
-			gs->strings[USB_GADGET_SERIAL_IDX].s = gs->serialnumber;
-			i++;
-		}
-		gi->gstrings[i] = NULL;
-		s = usb_gstrings_attach(&gi->cdev, gi->gstrings,
-				USB_GADGET_FIRST_AVAIL_IDX);
+		s = configfs_attach_gadget_strings(gi);
 		if (IS_ERR(s)) {
 			ret = PTR_ERR(s);
 			goto err_comp_cleanup;
@@ -1340,6 +1721,15 @@ static int configfs_composite_bind(struct usb_gadget *gadget,
 		gi->cdev.desc.iManufacturer = s[USB_GADGET_MANUFACTURER_IDX].id;
 		gi->cdev.desc.iProduct = s[USB_GADGET_PRODUCT_IDX].id;
 		gi->cdev.desc.iSerialNumber = s[USB_GADGET_SERIAL_IDX].id;
+
+		gi->cdev.usb_strings = s;
+	}
+
+	if (gi->use_webusb) {
+		cdev->use_webusb = true;
+		cdev->bcd_webusb_version = gi->bcd_webusb_version;
+		cdev->b_webusb_vendor_code = gi->b_webusb_vendor_code;
+		memcpy(cdev->landing_page, gi->landing_page, WEBUSB_URL_RAW_MAX_LENGTH);
 	}
 
 	if (gi->use_os_desc) {
@@ -1598,13 +1988,17 @@ static struct config_group *gadgets_make(
 	configfs_add_default_group(&gi->configs_group, &gi->group);
 
 	config_group_init_type_name(&gi->strings_group, "strings",
-			&gadget_strings_strings_type);
+			&gadget_language_strings_type);
 	configfs_add_default_group(&gi->strings_group, &gi->group);
 
 	config_group_init_type_name(&gi->os_desc_group, "os_desc",
 			&os_desc_type);
 	configfs_add_default_group(&gi->os_desc_group, &gi->group);
 
+	config_group_init_type_name(&gi->webusb_group, "webusb",
+			&webusb_type);
+	configfs_add_default_group(&gi->webusb_group, &gi->group);
+
 	gi->composite.bind = configfs_do_nothing;
 	gi->composite.unbind = configfs_do_nothing;
 	gi->composite.suspend = NULL;
diff --git a/drivers/usb/gadget/function/f_fs.c b/drivers/usb/gadget/function/f_fs.c
index 8ad354741380..ddfc537c7526 100644
--- a/drivers/usb/gadget/function/f_fs.c
+++ b/drivers/usb/gadget/function/f_fs.c
@@ -830,8 +830,7 @@ static void ffs_user_copy_worker(struct work_struct *work)
 {
 	struct ffs_io_data *io_data = container_of(work, struct ffs_io_data,
 						   work);
-	int ret = io_data->req->status ? io_data->req->status :
-					 io_data->req->actual;
+	int ret = io_data->status;
 	bool kiocb_has_eventfd = io_data->kiocb->ki_flags & IOCB_EVENTFD;
 
 	if (io_data->read && ret > 0) {
@@ -845,8 +844,6 @@ static void ffs_user_copy_worker(struct work_struct *work)
 	if (io_data->ffs->ffs_eventfd && !kiocb_has_eventfd)
 		eventfd_signal(io_data->ffs->ffs_eventfd, 1);
 
-	usb_ep_free_request(io_data->ep, io_data->req);
-
 	if (io_data->read)
 		kfree(io_data->to_free);
 	ffs_free_buffer(io_data);
@@ -861,6 +858,9 @@ static void ffs_epfile_async_io_complete(struct usb_ep *_ep,
 
 	ENTER();
 
+	io_data->status = req->status ? req->status : req->actual;
+	usb_ep_free_request(_ep, req);
+
 	INIT_WORK(&io_data->work, ffs_user_copy_worker);
 	queue_work(ffs->io_completion_wq, &io_data->work);
 }
diff --git a/drivers/usb/gadget/function/f_uvc.c b/drivers/usb/gadget/function/f_uvc.c
index 32f2c1645467..5e919fb65833 100644
--- a/drivers/usb/gadget/function/f_uvc.c
+++ b/drivers/usb/gadget/function/f_uvc.c
@@ -76,14 +76,14 @@ static struct usb_interface_descriptor uvc_control_intf = {
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= UVC_INTF_VIDEO_CONTROL,
 	.bAlternateSetting	= 0,
-	.bNumEndpoints		= 1,
+	.bNumEndpoints		= 0,
 	.bInterfaceClass	= USB_CLASS_VIDEO,
 	.bInterfaceSubClass	= UVC_SC_VIDEOCONTROL,
 	.bInterfaceProtocol	= 0x00,
 	.iInterface		= 0,
 };
 
-static struct usb_endpoint_descriptor uvc_control_ep = {
+static struct usb_endpoint_descriptor uvc_interrupt_ep = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 	.bEndpointAddress	= USB_DIR_IN,
@@ -92,8 +92,8 @@ static struct usb_endpoint_descriptor uvc_control_ep = {
 	.bInterval		= 8,
 };
 
-static struct usb_ss_ep_comp_descriptor uvc_ss_control_comp = {
-	.bLength		= sizeof(uvc_ss_control_comp),
+static struct usb_ss_ep_comp_descriptor uvc_ss_interrupt_comp = {
+	.bLength		= sizeof(uvc_ss_interrupt_comp),
 	.bDescriptorType	= USB_DT_SS_ENDPOINT_COMP,
 	/* The following 3 values can be tweaked if necessary. */
 	.bMaxBurst		= 0,
@@ -101,7 +101,7 @@ static struct usb_ss_ep_comp_descriptor uvc_ss_control_comp = {
 	.wBytesPerInterval	= cpu_to_le16(UVC_STATUS_MAX_PACKET_SIZE),
 };
 
-static struct uvc_control_endpoint_descriptor uvc_control_cs_ep = {
+static struct uvc_control_endpoint_descriptor uvc_interrupt_cs_ep = {
 	.bLength		= UVC_DT_CONTROL_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_CS_ENDPOINT,
 	.bDescriptorSubType	= UVC_EP_INTERRUPT,
@@ -300,14 +300,17 @@ uvc_function_set_alt(struct usb_function *f, unsigned interface, unsigned alt)
 		if (alt)
 			return -EINVAL;
 
-		uvcg_info(f, "reset UVC Control\n");
-		usb_ep_disable(uvc->control_ep);
+		if (uvc->enable_interrupt_ep) {
+			uvcg_info(f, "reset UVC interrupt endpoint\n");
+			usb_ep_disable(uvc->interrupt_ep);
 
-		if (!uvc->control_ep->desc)
-			if (config_ep_by_speed(cdev->gadget, f, uvc->control_ep))
-				return -EINVAL;
+			if (!uvc->interrupt_ep->desc)
+				if (config_ep_by_speed(cdev->gadget, f,
+						       uvc->interrupt_ep))
+					return -EINVAL;
 
-		usb_ep_enable(uvc->control_ep);
+			usb_ep_enable(uvc->interrupt_ep);
+		}
 
 		if (uvc->state == UVC_STATE_DISCONNECTED) {
 			memset(&v4l2_event, 0, sizeof(v4l2_event));
@@ -385,7 +388,8 @@ uvc_function_disable(struct usb_function *f)
 	uvc->state = UVC_STATE_DISCONNECTED;
 
 	usb_ep_disable(uvc->video.ep);
-	usb_ep_disable(uvc->control_ep);
+	if (uvc->enable_interrupt_ep)
+		usb_ep_disable(uvc->interrupt_ep);
 }
 
 /* --------------------------------------------------------------------------
@@ -474,6 +478,25 @@ uvc_register_video(struct uvc_device *uvc)
 		} \
 	} while (0)
 
+#define UVC_COPY_XU_DESCRIPTOR(mem, dst, desc)					\
+	do {									\
+		*(dst)++ = mem;							\
+		memcpy(mem, desc, 22); /* bLength to bNrInPins */		\
+		mem += 22;							\
+										\
+		memcpy(mem, (desc)->baSourceID, (desc)->bNrInPins);		\
+		mem += (desc)->bNrInPins;					\
+										\
+		memcpy(mem, &(desc)->bControlSize, 1);				\
+		mem++;								\
+										\
+		memcpy(mem, (desc)->bmControls, (desc)->bControlSize);		\
+		mem += (desc)->bControlSize;					\
+										\
+		memcpy(mem, &(desc)->iExtension, 1);				\
+		mem++;								\
+	} while (0)
+
 static struct usb_descriptor_header **
 uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 {
@@ -485,6 +508,7 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	const struct usb_descriptor_header * const *src;
 	struct usb_descriptor_header **dst;
 	struct usb_descriptor_header **hdr;
+	struct uvcg_extension *xu;
 	unsigned int control_size;
 	unsigned int streaming_size;
 	unsigned int n_desc;
@@ -521,9 +545,9 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	 * uvc_iad
 	 * uvc_control_intf
 	 * Class-specific UVC control descriptors
-	 * uvc_control_ep
-	 * uvc_control_cs_ep
-	 * uvc_ss_control_comp (for SS only)
+	 * uvc_interrupt_ep
+	 * uvc_interrupt_cs_ep
+	 * uvc_ss_interrupt_comp (for SS only)
 	 * uvc_streaming_intf_alt0
 	 * Class-specific UVC streaming descriptors
 	 * uvc_{fs|hs}_streaming
@@ -533,14 +557,17 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	control_size = 0;
 	streaming_size = 0;
 	bytes = uvc_iad.bLength + uvc_control_intf.bLength
-	      + uvc_control_ep.bLength + uvc_control_cs_ep.bLength
 	      + uvc_streaming_intf_alt0.bLength;
 
-	if (speed == USB_SPEED_SUPER) {
-		bytes += uvc_ss_control_comp.bLength;
-		n_desc = 6;
-	} else {
-		n_desc = 5;
+	n_desc = 3;
+	if (uvc->enable_interrupt_ep) {
+		bytes += uvc_interrupt_ep.bLength + uvc_interrupt_cs_ep.bLength;
+		n_desc += 2;
+
+		if (speed == USB_SPEED_SUPER) {
+			bytes += uvc_ss_interrupt_comp.bLength;
+			n_desc += 1;
+		}
 	}
 
 	for (src = (const struct usb_descriptor_header **)uvc_control_desc;
@@ -549,6 +576,13 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 		bytes += (*src)->bLength;
 		n_desc++;
 	}
+
+	list_for_each_entry(xu, uvc->desc.extension_units, list) {
+		control_size += xu->desc.bLength;
+		bytes += xu->desc.bLength;
+		n_desc++;
+	}
+
 	for (src = (const struct usb_descriptor_header **)uvc_streaming_cls;
 	     *src; ++src) {
 		streaming_size += (*src)->bLength;
@@ -575,15 +609,22 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	uvc_control_header = mem;
 	UVC_COPY_DESCRIPTORS(mem, dst,
 		(const struct usb_descriptor_header **)uvc_control_desc);
+
+	list_for_each_entry(xu, uvc->desc.extension_units, list)
+		UVC_COPY_XU_DESCRIPTOR(mem, dst, &xu->desc);
+
 	uvc_control_header->wTotalLength = cpu_to_le16(control_size);
 	uvc_control_header->bInCollection = 1;
 	uvc_control_header->baInterfaceNr[0] = uvc->streaming_intf;
 
-	UVC_COPY_DESCRIPTOR(mem, dst, &uvc_control_ep);
-	if (speed == USB_SPEED_SUPER)
-		UVC_COPY_DESCRIPTOR(mem, dst, &uvc_ss_control_comp);
+	if (uvc->enable_interrupt_ep) {
+		UVC_COPY_DESCRIPTOR(mem, dst, &uvc_interrupt_ep);
+		if (speed == USB_SPEED_SUPER)
+			UVC_COPY_DESCRIPTOR(mem, dst, &uvc_ss_interrupt_comp);
+
+		UVC_COPY_DESCRIPTOR(mem, dst, &uvc_interrupt_cs_ep);
+	}
 
-	UVC_COPY_DESCRIPTOR(mem, dst, &uvc_control_cs_ep);
 	UVC_COPY_DESCRIPTOR(mem, dst, &uvc_streaming_intf_alt0);
 
 	uvc_streaming_header = mem;
@@ -603,6 +644,7 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct usb_composite_dev *cdev = c->cdev;
 	struct uvc_device *uvc = to_uvc(f);
+	struct uvcg_extension *xu;
 	struct usb_string *us;
 	unsigned int max_packet_mult;
 	unsigned int max_packet_size;
@@ -666,12 +708,16 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 			    (opts->streaming_maxburst + 1));
 
 	/* Allocate endpoints. */
-	ep = usb_ep_autoconfig(cdev->gadget, &uvc_control_ep);
-	if (!ep) {
-		uvcg_info(f, "Unable to allocate control EP\n");
-		goto error;
+	if (opts->enable_interrupt_ep) {
+		ep = usb_ep_autoconfig(cdev->gadget, &uvc_interrupt_ep);
+		if (!ep) {
+			uvcg_info(f, "Unable to allocate interrupt EP\n");
+			goto error;
+		}
+		uvc->interrupt_ep = ep;
+		uvc_control_intf.bNumEndpoints = 1;
 	}
-	uvc->control_ep = ep;
+	uvc->enable_interrupt_ep = opts->enable_interrupt_ep;
 
 	if (gadget_is_superspeed(c->cdev->gadget))
 		ep = usb_ep_autoconfig_ss(cdev->gadget, &uvc_ss_streaming_ep,
@@ -691,6 +737,18 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	uvc_hs_streaming_ep.bEndpointAddress = uvc->video.ep->address;
 	uvc_ss_streaming_ep.bEndpointAddress = uvc->video.ep->address;
 
+	/*
+	 * XUs can have an arbitrary string descriptor describing them. If they
+	 * have one pick up the ID.
+	 */
+	list_for_each_entry(xu, &opts->extension_units, list)
+		if (xu->string_descriptor_index)
+			xu->desc.iExtension = cdev->usb_strings[xu->string_descriptor_index].id;
+
+	/*
+	 * We attach the hard-coded defaults incase the user does not provide
+	 * any more appropriate strings through configfs.
+	 */
 	uvc_en_us_strings[UVC_STRING_CONTROL_IDX].s = opts->function_name;
 	us = usb_gstrings_attach(cdev, uvc_function_strings,
 				 ARRAY_SIZE(uvc_en_us_strings));
@@ -698,11 +756,15 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 		ret = PTR_ERR(us);
 		goto error;
 	}
-	uvc_iad.iFunction = us[UVC_STRING_CONTROL_IDX].id;
-	uvc_control_intf.iInterface = us[UVC_STRING_CONTROL_IDX].id;
-	ret = us[UVC_STRING_STREAMING_IDX].id;
-	uvc_streaming_intf_alt0.iInterface = ret;
-	uvc_streaming_intf_alt1.iInterface = ret;
+
+	uvc_iad.iFunction = opts->iad_index ? cdev->usb_strings[opts->iad_index].id :
+			    us[UVC_STRING_CONTROL_IDX].id;
+	uvc_streaming_intf_alt0.iInterface = opts->vs0_index ?
+					     cdev->usb_strings[opts->vs0_index].id :
+					     us[UVC_STRING_STREAMING_IDX].id;
+	uvc_streaming_intf_alt1.iInterface = opts->vs1_index ?
+					     cdev->usb_strings[opts->vs1_index].id :
+					     us[UVC_STRING_STREAMING_IDX].id;
 
 	/* Allocate interface IDs. */
 	if ((ret = usb_interface_id(c, f)) < 0)
@@ -803,7 +865,6 @@ static struct usb_function_instance *uvc_alloc_inst(void)
 	struct uvc_camera_terminal_descriptor *cd;
 	struct uvc_processing_unit_descriptor *pd;
 	struct uvc_output_terminal_descriptor *od;
-	struct uvc_color_matching_descriptor *md;
 	struct uvc_descriptor_header **ctl_cls;
 	int ret;
 
@@ -852,13 +913,12 @@ static struct usb_function_instance *uvc_alloc_inst(void)
 	od->bSourceID			= 2;
 	od->iTerminal			= 0;
 
-	md = &opts->uvc_color_matching;
-	md->bLength			= UVC_DT_COLOR_MATCHING_SIZE;
-	md->bDescriptorType		= USB_DT_CS_INTERFACE;
-	md->bDescriptorSubType		= UVC_VS_COLORFORMAT;
-	md->bColorPrimaries		= 1;
-	md->bTransferCharacteristics	= 1;
-	md->bMatrixCoefficients		= 4;
+	/*
+	 * With the ability to add XUs to the UVC function graph, we need to be
+	 * able to allocate unique unit IDs to them. The IDs are 1-based, with
+	 * the CT, PU and OT above consuming the first 3.
+	 */
+	opts->last_unit_id		= 3;
 
 	/* Prepare fs control class descriptors for configfs-based gadgets */
 	ctl_cls = opts->uvc_fs_control_cls;
@@ -880,6 +940,8 @@ static struct usb_function_instance *uvc_alloc_inst(void)
 	opts->ss_control =
 		(const struct uvc_descriptor_header * const *)ctl_cls;
 
+	INIT_LIST_HEAD(&opts->extension_units);
+
 	opts->streaming_interval = 1;
 	opts->streaming_maxpacket = 1024;
 	snprintf(opts->function_name, sizeof(opts->function_name), "UVC Camera");
@@ -1011,6 +1073,8 @@ static struct usb_function *uvc_alloc(struct usb_function_instance *fi)
 		return ERR_PTR(-EBUSY);
 	}
 
+	uvc->desc.extension_units = &opts->extension_units;
+
 	++opts->refcnt;
 	mutex_unlock(&opts->lock);
 
diff --git a/drivers/usb/gadget/function/u_ether.c b/drivers/usb/gadget/function/u_ether.c
index e06022873df1..f259975dfba4 100644
--- a/drivers/usb/gadget/function/u_ether.c
+++ b/drivers/usb/gadget/function/u_ether.c
@@ -17,6 +17,7 @@
 #include <linux/etherdevice.h>
 #include <linux/ethtool.h>
 #include <linux/if_vlan.h>
+#include <linux/usb/composite.h>
 
 #include "u_ether.h"
 
@@ -103,41 +104,6 @@ static inline int qlen(struct usb_gadget *gadget, unsigned qmult)
 
 /*-------------------------------------------------------------------------*/
 
-/* REVISIT there must be a better way than having two sets
- * of debug calls ...
- */
-
-#undef DBG
-#undef VDBG
-#undef ERROR
-#undef INFO
-
-#define xprintk(d, level, fmt, args...) \
-	printk(level "%s: " fmt , (d)->net->name , ## args)
-
-#ifdef DEBUG
-#undef DEBUG
-#define DBG(dev, fmt, args...) \
-	xprintk(dev , KERN_DEBUG , fmt , ## args)
-#else
-#define DBG(dev, fmt, args...) \
-	do { } while (0)
-#endif /* DEBUG */
-
-#ifdef VERBOSE_DEBUG
-#define VDBG	DBG
-#else
-#define VDBG(dev, fmt, args...) \
-	do { } while (0)
-#endif /* DEBUG */
-
-#define ERROR(dev, fmt, args...) \
-	xprintk(dev , KERN_ERR , fmt , ## args)
-#define INFO(dev, fmt, args...) \
-	xprintk(dev , KERN_INFO , fmt , ## args)
-
-/*-------------------------------------------------------------------------*/
-
 /* NETWORK DRIVER HOOKUP (to the layer above this driver) */
 
 static void eth_get_drvinfo(struct net_device *net, struct ethtool_drvinfo *p)
@@ -846,13 +812,11 @@ struct net_device *gether_setup_name_default(const char *netname)
 	snprintf(net->name, sizeof(net->name), "%s%%d", netname);
 
 	eth_random_addr(dev->dev_mac);
-	pr_warn("using random %s ethernet address\n", "self");
 
 	/* by default we always have a random MAC address */
 	net->addr_assign_type = NET_ADDR_RANDOM;
 
 	eth_random_addr(dev->host_mac);
-	pr_warn("using random %s ethernet address\n", "host");
 
 	net->netdev_ops = &eth_netdev_ops;
 
diff --git a/drivers/usb/gadget/function/u_serial.c b/drivers/usb/gadget/function/u_serial.c
index 840626e064e1..a0ca47fbff0f 100644
--- a/drivers/usb/gadget/function/u_serial.c
+++ b/drivers/usb/gadget/function/u_serial.c
@@ -82,6 +82,9 @@
 #define WRITE_BUF_SIZE		8192		/* TX only */
 #define GS_CONSOLE_BUF_SIZE	8192
 
+/* Prevents race conditions while accessing gser->ioport */
+static DEFINE_SPINLOCK(serial_port_lock);
+
 /* console info */
 struct gs_console {
 	struct console		console;
@@ -1375,8 +1378,10 @@ void gserial_disconnect(struct gserial *gser)
 	if (!port)
 		return;
 
+	spin_lock_irqsave(&serial_port_lock, flags);
+
 	/* tell the TTY glue not to do I/O here any more */
-	spin_lock_irqsave(&port->port_lock, flags);
+	spin_lock(&port->port_lock);
 
 	gs_console_disconnect(port);
 
@@ -1391,7 +1396,8 @@ void gserial_disconnect(struct gserial *gser)
 			tty_hangup(port->port.tty);
 	}
 	port->suspended = false;
-	spin_unlock_irqrestore(&port->port_lock, flags);
+	spin_unlock(&port->port_lock);
+	spin_unlock_irqrestore(&serial_port_lock, flags);
 
 	/* disable endpoints, aborting down any active I/O */
 	usb_ep_disable(gser->out);
@@ -1425,10 +1431,19 @@ EXPORT_SYMBOL_GPL(gserial_suspend);
 
 void gserial_resume(struct gserial *gser)
 {
-	struct gs_port *port = gser->ioport;
+	struct gs_port *port;
 	unsigned long	flags;
 
-	spin_lock_irqsave(&port->port_lock, flags);
+	spin_lock_irqsave(&serial_port_lock, flags);
+	port = gser->ioport;
+
+	if (!port) {
+		spin_unlock_irqrestore(&serial_port_lock, flags);
+		return;
+	}
+
+	spin_lock(&port->port_lock);
+	spin_unlock(&serial_port_lock);
 	port->suspended = false;
 	if (!port->start_delayed) {
 		spin_unlock_irqrestore(&port->port_lock, flags);
diff --git a/drivers/usb/gadget/function/u_uvc.h b/drivers/usb/gadget/function/u_uvc.h
index 24b8681b0d6f..1ce58f61253c 100644
--- a/drivers/usb/gadget/function/u_uvc.h
+++ b/drivers/usb/gadget/function/u_uvc.h
@@ -28,6 +28,9 @@ struct f_uvc_opts {
 	unsigned int					control_interface;
 	unsigned int					streaming_interface;
 	char						function_name[32];
+	unsigned int					last_unit_id;
+
+	bool						enable_interrupt_ep;
 
 	/*
 	 * Control descriptors array pointers for full-/high-speed and
@@ -52,7 +55,6 @@ struct f_uvc_opts {
 	struct uvc_camera_terminal_descriptor		uvc_camera_terminal;
 	struct uvc_processing_unit_descriptor		uvc_processing;
 	struct uvc_output_terminal_descriptor		uvc_output_terminal;
-	struct uvc_color_matching_descriptor		uvc_color_matching;
 
 	/*
 	 * Control descriptors pointers arrays for full-/high-speed and
@@ -65,6 +67,12 @@ struct f_uvc_opts {
 	struct uvc_descriptor_header			*uvc_ss_control_cls[5];
 
 	/*
+	 * Control descriptors for extension units. There could be any number
+	 * of these, including none at all.
+	 */
+	struct list_head				extension_units;
+
+	/*
 	 * Streaming descriptors for full-speed, high-speed and super-speed.
 	 * Used by configfs only, must not be touched by legacy gadgets. The
 	 * arrays are allocated at runtime as the number of descriptors isn't
@@ -75,6 +83,14 @@ struct f_uvc_opts {
 	struct uvc_descriptor_header			**uvc_ss_streaming_cls;
 
 	/*
+	 * Indexes into the function's string descriptors allowing users to set
+	 * custom descriptions rather than the hard-coded defaults.
+	 */
+	u8						iad_index;
+	u8						vs0_index;
+	u8						vs1_index;
+
+	/*
 	 * Read/write access to configfs attributes is handled by configfs.
 	 *
 	 * This lock protects the descriptors from concurrent access by
diff --git a/drivers/usb/gadget/function/uvc.h b/drivers/usb/gadget/function/uvc.h
index 40226b1f7e14..100475b1363e 100644
--- a/drivers/usb/gadget/function/uvc.h
+++ b/drivers/usb/gadget/function/uvc.h
@@ -143,12 +143,14 @@ struct uvc_device {
 		const struct uvc_descriptor_header * const *fs_streaming;
 		const struct uvc_descriptor_header * const *hs_streaming;
 		const struct uvc_descriptor_header * const *ss_streaming;
+		struct list_head *extension_units;
 	} desc;
 
 	unsigned int control_intf;
-	struct usb_ep *control_ep;
+	struct usb_ep *interrupt_ep;
 	struct usb_request *control_req;
 	void *control_buf;
+	bool enable_interrupt_ep;
 
 	unsigned int streaming_intf;
 
diff --git a/drivers/usb/gadget/function/uvc_configfs.c b/drivers/usb/gadget/function/uvc_configfs.c
index 76cb60d13049..62b759bb7613 100644
--- a/drivers/usb/gadget/function/uvc_configfs.c
+++ b/drivers/usb/gadget/function/uvc_configfs.c
@@ -13,6 +13,7 @@
 #include "uvc_configfs.h"
 
 #include <linux/sort.h>
+#include <linux/usb/video.h>
 
 /* -----------------------------------------------------------------------------
  * Global Utility Structures and Macros
@@ -46,6 +47,71 @@ static int uvcg_config_compare_u32(const void *l, const void *r)
 	return li < ri ? -1 : li == ri ? 0 : 1;
 }
 
+static inline int __uvcg_count_item_entries(char *buf, void *priv, unsigned int size)
+{
+	++*((int *)priv);
+	return 0;
+}
+
+static inline int __uvcg_fill_item_entries(char *buf, void *priv, unsigned int size)
+{
+	unsigned int num;
+	u8 **values;
+	int ret;
+
+	ret = kstrtouint(buf, 0, &num);
+	if (ret)
+		return ret;
+
+	if (num != (num & GENMASK((size * 8) - 1, 0)))
+		return -ERANGE;
+
+	values = priv;
+	memcpy(*values, &num, size);
+	*values += size;
+
+	return 0;
+}
+
+static int __uvcg_iter_item_entries(const char *page, size_t len,
+				    int (*fun)(char *, void *, unsigned int),
+				    void *priv, unsigned int size)
+{
+	/* sign, base 2 representation, newline, terminator */
+	unsigned int bufsize = 1 + size * 8 + 1 + 1;
+	const char *pg = page;
+	int i, ret = 0;
+	char *buf;
+
+	if (!fun)
+		return -EINVAL;
+
+	buf = kzalloc(bufsize, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	while (pg - page < len) {
+		i = 0;
+		while (i < sizeof(buf) && (pg - page < len) &&
+		       *pg != '\0' && *pg != '\n')
+			buf[i++] = *pg++;
+		if (i == sizeof(buf)) {
+			ret = -EINVAL;
+			goto out_free_buf;
+		}
+		while ((pg - page < len) && (*pg == '\0' || *pg == '\n'))
+			++pg;
+		buf[i] = '\0';
+		ret = fun(buf, priv, size);
+		if (ret)
+			goto out_free_buf;
+	}
+
+out_free_buf:
+	kfree(buf);
+	return ret;
+}
+
 struct uvcg_config_group_type {
 	struct config_item_type type;
 	const char *name;
@@ -483,11 +549,68 @@ UVC_ATTR_RO(uvcg_default_output_, cname, aname)
 UVCG_DEFAULT_OUTPUT_ATTR(b_terminal_id, bTerminalID, 8);
 UVCG_DEFAULT_OUTPUT_ATTR(w_terminal_type, wTerminalType, 16);
 UVCG_DEFAULT_OUTPUT_ATTR(b_assoc_terminal, bAssocTerminal, 8);
-UVCG_DEFAULT_OUTPUT_ATTR(b_source_id, bSourceID, 8);
 UVCG_DEFAULT_OUTPUT_ATTR(i_terminal, iTerminal, 8);
 
 #undef UVCG_DEFAULT_OUTPUT_ATTR
 
+static ssize_t uvcg_default_output_b_source_id_show(struct config_item *item,
+						    char *page)
+{
+	struct config_group *group = to_config_group(item);
+	struct f_uvc_opts *opts;
+	struct config_item *opts_item;
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvc_output_terminal_descriptor *cd;
+	int result;
+
+	mutex_lock(su_mutex); /* for navigating configfs hierarchy */
+
+	opts_item = group->cg_item.ci_parent->ci_parent->
+			ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+	cd = &opts->uvc_output_terminal;
+
+	mutex_lock(&opts->lock);
+	result = sprintf(page, "%u\n", le8_to_cpu(cd->bSourceID));
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+
+	return result;
+}
+
+static ssize_t uvcg_default_output_b_source_id_store(struct config_item *item,
+						     const char *page, size_t len)
+{
+	struct config_group *group = to_config_group(item);
+	struct f_uvc_opts *opts;
+	struct config_item *opts_item;
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvc_output_terminal_descriptor *cd;
+	int result;
+	u8 num;
+
+	result = kstrtou8(page, 0, &num);
+	if (result)
+		return result;
+
+	mutex_lock(su_mutex); /* for navigating configfs hierarchy */
+
+	opts_item = group->cg_item.ci_parent->ci_parent->
+			ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+	cd = &opts->uvc_output_terminal;
+
+	mutex_lock(&opts->lock);
+	cd->bSourceID = num;
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+
+	return len;
+}
+UVC_ATTR(uvcg_default_output_, b_source_id, bSourceID);
+
 static struct configfs_attribute *uvcg_default_output_attrs[] = {
 	&uvcg_default_output_attr_b_terminal_id,
 	&uvcg_default_output_attr_w_terminal_type,
@@ -540,6 +663,537 @@ static const struct uvcg_config_group_type uvcg_terminal_grp_type = {
 };
 
 /* -----------------------------------------------------------------------------
+ * control/extensions
+ */
+
+#define UVCG_EXTENSION_ATTR(cname, aname, ro...)			\
+static ssize_t uvcg_extension_##cname##_show(struct config_item *item,	\
+					     char *page)		\
+{									\
+	struct config_group *group = to_config_group(item->ci_parent);	\
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;		\
+	struct uvcg_extension *xu = to_uvcg_extension(item);		\
+	struct config_item *opts_item;					\
+	struct f_uvc_opts *opts;					\
+	int ret;							\
+									\
+	mutex_lock(su_mutex);						\
+									\
+	opts_item = item->ci_parent->ci_parent->ci_parent;		\
+	opts = to_f_uvc_opts(opts_item);				\
+									\
+	mutex_lock(&opts->lock);					\
+	ret = sprintf(page, "%u\n", xu->desc.aname);			\
+	mutex_unlock(&opts->lock);					\
+									\
+	mutex_unlock(su_mutex);						\
+									\
+	return ret;							\
+}									\
+UVC_ATTR##ro(uvcg_extension_, cname, aname)
+
+UVCG_EXTENSION_ATTR(b_length, bLength, _RO);
+UVCG_EXTENSION_ATTR(b_unit_id, bUnitID, _RO);
+UVCG_EXTENSION_ATTR(i_extension, iExtension, _RO);
+
+static ssize_t uvcg_extension_b_num_controls_store(struct config_item *item,
+						   const char *page, size_t len)
+{
+	struct config_group *group = to_config_group(item->ci_parent);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	int ret;
+	u8 num;
+
+	ret = kstrtou8(page, 0, &num);
+	if (ret)
+		return ret;
+
+	mutex_lock(su_mutex);
+
+	opts_item = item->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+	xu->desc.bNumControls = num;
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+
+	return len;
+}
+UVCG_EXTENSION_ATTR(b_num_controls, bNumControls);
+
+/*
+ * In addition to storing bNrInPins, this function needs to realloc the
+ * memory for the baSourceID array and additionally expand bLength.
+ */
+static ssize_t uvcg_extension_b_nr_in_pins_store(struct config_item *item,
+						 const char *page, size_t len)
+{
+	struct config_group *group = to_config_group(item->ci_parent);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	void *tmp_buf;
+	int ret;
+	u8 num;
+
+	ret = kstrtou8(page, 0, &num);
+	if (ret)
+		return ret;
+
+	mutex_lock(su_mutex);
+
+	opts_item = item->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+
+	if (num == xu->desc.bNrInPins) {
+		ret = len;
+		goto unlock;
+	}
+
+	tmp_buf = krealloc_array(xu->desc.baSourceID, num, sizeof(u8),
+				 GFP_KERNEL | __GFP_ZERO);
+	if (!tmp_buf) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	xu->desc.baSourceID = tmp_buf;
+	xu->desc.bNrInPins = num;
+	xu->desc.bLength = UVC_DT_EXTENSION_UNIT_SIZE(xu->desc.bNrInPins,
+						      xu->desc.bControlSize);
+
+	ret = len;
+
+unlock:
+	mutex_unlock(&opts->lock);
+	mutex_unlock(su_mutex);
+	return ret;
+}
+UVCG_EXTENSION_ATTR(b_nr_in_pins, bNrInPins);
+
+/*
+ * In addition to storing bControlSize, this function needs to realloc the
+ * memory for the bmControls array and additionally expand bLength.
+ */
+static ssize_t uvcg_extension_b_control_size_store(struct config_item *item,
+						   const char *page, size_t len)
+{
+	struct config_group *group = to_config_group(item->ci_parent);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	void *tmp_buf;
+	int ret;
+	u8 num;
+
+	ret = kstrtou8(page, 0, &num);
+	if (ret)
+		return ret;
+
+	mutex_lock(su_mutex);
+
+	opts_item = item->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+
+	if (num == xu->desc.bControlSize) {
+		ret = len;
+		goto unlock;
+	}
+
+	tmp_buf = krealloc_array(xu->desc.bmControls, num, sizeof(u8),
+				 GFP_KERNEL | __GFP_ZERO);
+	if (!tmp_buf) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	xu->desc.bmControls = tmp_buf;
+	xu->desc.bControlSize = num;
+	xu->desc.bLength = UVC_DT_EXTENSION_UNIT_SIZE(xu->desc.bNrInPins,
+						      xu->desc.bControlSize);
+
+	ret = len;
+
+unlock:
+	mutex_unlock(&opts->lock);
+	mutex_unlock(su_mutex);
+	return ret;
+}
+
+UVCG_EXTENSION_ATTR(b_control_size, bControlSize);
+
+static ssize_t uvcg_extension_guid_extension_code_show(struct config_item *item,
+						       char *page)
+{
+	struct config_group *group = to_config_group(item->ci_parent);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+
+	mutex_lock(su_mutex);
+
+	opts_item = item->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+	memcpy(page, xu->desc.guidExtensionCode, sizeof(xu->desc.guidExtensionCode));
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+
+	return sizeof(xu->desc.guidExtensionCode);
+}
+
+static ssize_t uvcg_extension_guid_extension_code_store(struct config_item *item,
+							const char *page, size_t len)
+{
+	struct config_group *group = to_config_group(item->ci_parent);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	int ret;
+
+	mutex_lock(su_mutex);
+
+	opts_item = item->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+	memcpy(xu->desc.guidExtensionCode, page,
+	       min(sizeof(xu->desc.guidExtensionCode), len));
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+
+	ret = sizeof(xu->desc.guidExtensionCode);
+
+	return ret;
+}
+
+UVC_ATTR(uvcg_extension_, guid_extension_code, guidExtensionCode);
+
+static ssize_t uvcg_extension_ba_source_id_show(struct config_item *item,
+						char *page)
+{
+	struct config_group *group = to_config_group(item->ci_parent);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	char *pg = page;
+	int ret, i;
+
+	mutex_lock(su_mutex);
+
+	opts_item = item->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+	for (ret = 0, i = 0; i < xu->desc.bNrInPins; ++i) {
+		ret += sprintf(pg, "%u\n", xu->desc.baSourceID[i]);
+		pg = page + ret;
+	}
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+
+	return ret;
+}
+
+static ssize_t uvcg_extension_ba_source_id_store(struct config_item *item,
+						 const char *page, size_t len)
+{
+	struct config_group *group = to_config_group(item->ci_parent);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	u8 *source_ids, *iter;
+	int ret, n = 0;
+
+	mutex_lock(su_mutex);
+
+	opts_item = item->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+
+	ret = __uvcg_iter_item_entries(page, len, __uvcg_count_item_entries, &n,
+				       sizeof(u8));
+	if (ret)
+		goto unlock;
+
+	iter = source_ids = kcalloc(n, sizeof(u8), GFP_KERNEL);
+	if (!source_ids) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	ret = __uvcg_iter_item_entries(page, len, __uvcg_fill_item_entries, &iter,
+				       sizeof(u8));
+	if (ret) {
+		kfree(source_ids);
+		goto unlock;
+	}
+
+	kfree(xu->desc.baSourceID);
+	xu->desc.baSourceID = source_ids;
+	xu->desc.bNrInPins = n;
+	xu->desc.bLength = UVC_DT_EXTENSION_UNIT_SIZE(xu->desc.bNrInPins,
+						      xu->desc.bControlSize);
+
+	ret = len;
+
+unlock:
+	mutex_unlock(&opts->lock);
+	mutex_unlock(su_mutex);
+	return ret;
+}
+UVC_ATTR(uvcg_extension_, ba_source_id, baSourceID);
+
+static ssize_t uvcg_extension_bm_controls_show(struct config_item *item,
+					       char *page)
+{
+	struct config_group *group = to_config_group(item->ci_parent);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	char *pg = page;
+	int ret, i;
+
+	mutex_lock(su_mutex);
+
+	opts_item = item->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+	for (ret = 0, i = 0; i < xu->desc.bControlSize; ++i) {
+		ret += sprintf(pg, "0x%02x\n", xu->desc.bmControls[i]);
+		pg = page + ret;
+	}
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+
+	return ret;
+}
+
+static ssize_t uvcg_extension_bm_controls_store(struct config_item *item,
+						const char *page, size_t len)
+{
+	struct config_group *group = to_config_group(item->ci_parent);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	u8 *bm_controls, *iter;
+	int ret, n = 0;
+
+	mutex_lock(su_mutex);
+
+	opts_item = item->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+
+	ret = __uvcg_iter_item_entries(page, len, __uvcg_count_item_entries, &n,
+				       sizeof(u8));
+	if (ret)
+		goto unlock;
+
+	iter = bm_controls = kcalloc(n, sizeof(u8), GFP_KERNEL);
+	if (!bm_controls) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	ret = __uvcg_iter_item_entries(page, len, __uvcg_fill_item_entries, &iter,
+				       sizeof(u8));
+	if (ret) {
+		kfree(bm_controls);
+		goto unlock;
+	}
+
+	kfree(xu->desc.bmControls);
+	xu->desc.bmControls = bm_controls;
+	xu->desc.bControlSize = n;
+	xu->desc.bLength = UVC_DT_EXTENSION_UNIT_SIZE(xu->desc.bNrInPins,
+						      xu->desc.bControlSize);
+
+	ret = len;
+
+unlock:
+	mutex_unlock(&opts->lock);
+	mutex_unlock(su_mutex);
+	return ret;
+}
+
+UVC_ATTR(uvcg_extension_, bm_controls, bmControls);
+
+static struct configfs_attribute *uvcg_extension_attrs[] = {
+	&uvcg_extension_attr_b_length,
+	&uvcg_extension_attr_b_unit_id,
+	&uvcg_extension_attr_b_num_controls,
+	&uvcg_extension_attr_b_nr_in_pins,
+	&uvcg_extension_attr_b_control_size,
+	&uvcg_extension_attr_guid_extension_code,
+	&uvcg_extension_attr_ba_source_id,
+	&uvcg_extension_attr_bm_controls,
+	&uvcg_extension_attr_i_extension,
+	NULL,
+};
+
+static void uvcg_extension_release(struct config_item *item)
+{
+	struct uvcg_extension *xu = container_of(item, struct uvcg_extension, item);
+
+	kfree(xu);
+}
+
+static int uvcg_extension_allow_link(struct config_item *src, struct config_item *tgt)
+{
+	struct mutex *su_mutex = &src->ci_group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(src);
+	struct config_item *gadget_item;
+	struct gadget_string *string;
+	struct config_item *strings;
+	int ret = 0;
+
+	mutex_lock(su_mutex); /* for navigating configfs hierarchy */
+
+	/* Validate that the target of the link is an entry in strings/<langid> */
+	gadget_item = src->ci_parent->ci_parent->ci_parent->ci_parent->ci_parent;
+	strings = config_group_find_item(to_config_group(gadget_item), "strings");
+	if (!strings || tgt->ci_parent->ci_parent != strings) {
+		ret = -EINVAL;
+		goto put_strings;
+	}
+
+	string = to_gadget_string(tgt);
+	xu->string_descriptor_index = string->usb_string.id;
+
+put_strings:
+	config_item_put(strings);
+	mutex_unlock(su_mutex);
+
+	return ret;
+}
+
+static void uvcg_extension_drop_link(struct config_item *src, struct config_item *tgt)
+{
+	struct mutex *su_mutex = &src->ci_group->cg_subsys->su_mutex;
+	struct uvcg_extension *xu = to_uvcg_extension(src);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+
+	mutex_lock(su_mutex); /* for navigating configfs hierarchy */
+
+	opts_item = src->ci_parent->ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+
+	xu->string_descriptor_index = 0;
+
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+}
+
+static struct configfs_item_operations uvcg_extension_item_ops = {
+	.release	= uvcg_extension_release,
+	.allow_link	= uvcg_extension_allow_link,
+	.drop_link	= uvcg_extension_drop_link,
+};
+
+static const struct config_item_type uvcg_extension_type = {
+	.ct_item_ops	= &uvcg_extension_item_ops,
+	.ct_attrs	= uvcg_extension_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static void uvcg_extension_drop(struct config_group *group, struct config_item *item)
+{
+	struct uvcg_extension *xu = container_of(item, struct uvcg_extension, item);
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+
+	opts_item = group->cg_item.ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+
+	config_item_put(item);
+	list_del(&xu->list);
+	kfree(xu->desc.baSourceID);
+	kfree(xu->desc.bmControls);
+
+	mutex_unlock(&opts->lock);
+}
+
+static struct config_item *uvcg_extension_make(struct config_group *group, const char *name)
+{
+	struct config_item *opts_item;
+	struct uvcg_extension *xu;
+	struct f_uvc_opts *opts;
+
+	opts_item = group->cg_item.ci_parent->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	xu = kzalloc(sizeof(*xu), GFP_KERNEL);
+	if (!xu)
+		return ERR_PTR(-ENOMEM);
+
+	xu->desc.bLength = UVC_DT_EXTENSION_UNIT_SIZE(0, 0);
+	xu->desc.bDescriptorType = USB_DT_CS_INTERFACE;
+	xu->desc.bDescriptorSubType = UVC_VC_EXTENSION_UNIT;
+	xu->desc.bNumControls = 0;
+	xu->desc.bNrInPins = 0;
+	xu->desc.baSourceID = NULL;
+	xu->desc.bControlSize = 0;
+	xu->desc.bmControls = NULL;
+
+	mutex_lock(&opts->lock);
+
+	xu->desc.bUnitID = ++opts->last_unit_id;
+
+	config_item_init_type_name(&xu->item, name, &uvcg_extension_type);
+	list_add_tail(&xu->list, &opts->extension_units);
+
+	mutex_unlock(&opts->lock);
+
+	return &xu->item;
+}
+
+static struct configfs_group_operations uvcg_extensions_grp_ops = {
+	.make_item	= uvcg_extension_make,
+	.drop_item	= uvcg_extension_drop,
+};
+
+static const struct uvcg_config_group_type uvcg_extensions_grp_type = {
+	.type = {
+		.ct_item_ops	= &uvcg_config_item_ops,
+		.ct_group_ops	= &uvcg_extensions_grp_ops,
+		.ct_owner	= THIS_MODULE,
+	},
+	.name = "extensions",
+};
+
+/* -----------------------------------------------------------------------------
  * control/class/{fs|ss}
  */
 
@@ -716,8 +1370,61 @@ static ssize_t uvcg_default_control_b_interface_number_show(
 
 UVC_ATTR_RO(uvcg_default_control_, b_interface_number, bInterfaceNumber);
 
+static ssize_t uvcg_default_control_enable_interrupt_ep_show(
+	struct config_item *item, char *page)
+{
+	struct config_group *group = to_config_group(item);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	int result = 0;
+
+	mutex_lock(su_mutex); /* for navigating configfs hierarchy */
+
+	opts_item = item->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+	result += sprintf(page, "%u\n", opts->enable_interrupt_ep);
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+
+	return result;
+}
+
+static ssize_t uvcg_default_control_enable_interrupt_ep_store(
+	struct config_item *item, const char *page, size_t len)
+{
+	struct config_group *group = to_config_group(item);
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;
+	struct config_item *opts_item;
+	struct f_uvc_opts *opts;
+	ssize_t ret;
+	u8 num;
+
+	ret = kstrtou8(page, 0, &num);
+	if (ret)
+		return ret;
+
+	mutex_lock(su_mutex); /* for navigating configfs hierarchy */
+
+	opts_item = item->ci_parent;
+	opts = to_f_uvc_opts(opts_item);
+
+	mutex_lock(&opts->lock);
+	opts->enable_interrupt_ep = num;
+	mutex_unlock(&opts->lock);
+
+	mutex_unlock(su_mutex);
+
+	return len;
+}
+UVC_ATTR(uvcg_default_control_, enable_interrupt_ep, enable_interrupt_ep);
+
 static struct configfs_attribute *uvcg_default_control_attrs[] = {
 	&uvcg_default_control_attr_b_interface_number,
+	&uvcg_default_control_attr_enable_interrupt_ep,
 	NULL,
 };
 
@@ -733,6 +1440,7 @@ static const struct uvcg_config_group_type uvcg_control_grp_type = {
 		&uvcg_processing_grp_type,
 		&uvcg_terminal_grp_type,
 		&uvcg_control_class_grp_type,
+		&uvcg_extensions_grp_type,
 		NULL,
 	},
 };
@@ -747,6 +1455,100 @@ static const char * const uvcg_format_names[] = {
 	"mjpeg",
 };
 
+static struct uvcg_color_matching *
+uvcg_format_get_default_color_match(struct config_item *streaming)
+{
+	struct config_item *color_matching_item, *cm_default;
+	struct uvcg_color_matching *color_match;
+
+	color_matching_item = config_group_find_item(to_config_group(streaming),
+						     "color_matching");
+	if (!color_matching_item)
+		return NULL;
+
+	cm_default = config_group_find_item(to_config_group(color_matching_item),
+					    "default");
+	config_item_put(color_matching_item);
+	if (!cm_default)
+		return NULL;
+
+	color_match = to_uvcg_color_matching(to_config_group(cm_default));
+	config_item_put(cm_default);
+
+	return color_match;
+}
+
+static int uvcg_format_allow_link(struct config_item *src, struct config_item *tgt)
+{
+	struct mutex *su_mutex = &src->ci_group->cg_subsys->su_mutex;
+	struct uvcg_color_matching *color_matching_desc;
+	struct config_item *streaming, *color_matching;
+	struct uvcg_format *fmt;
+	int ret = 0;
+
+	mutex_lock(su_mutex);
+
+	streaming = src->ci_parent->ci_parent;
+	color_matching = config_group_find_item(to_config_group(streaming), "color_matching");
+	if (!color_matching || color_matching != tgt->ci_parent) {
+		ret = -EINVAL;
+		goto out_put_cm;
+	}
+
+	fmt = to_uvcg_format(src);
+
+	/*
+	 * There's always a color matching descriptor associated with the format
+	 * but without a symlink it should only ever be the default one. If it's
+	 * not the default, there's already a symlink and we should bail out.
+	 */
+	color_matching_desc = uvcg_format_get_default_color_match(streaming);
+	if (fmt->color_matching != color_matching_desc) {
+		ret = -EBUSY;
+		goto out_put_cm;
+	}
+
+	color_matching_desc->refcnt--;
+
+	color_matching_desc = to_uvcg_color_matching(to_config_group(tgt));
+	fmt->color_matching = color_matching_desc;
+	color_matching_desc->refcnt++;
+
+out_put_cm:
+	config_item_put(color_matching);
+	mutex_unlock(su_mutex);
+
+	return ret;
+}
+
+static void uvcg_format_drop_link(struct config_item *src, struct config_item *tgt)
+{
+	struct mutex *su_mutex = &src->ci_group->cg_subsys->su_mutex;
+	struct uvcg_color_matching *color_matching_desc;
+	struct config_item *streaming;
+	struct uvcg_format *fmt;
+
+	mutex_lock(su_mutex);
+
+	color_matching_desc = to_uvcg_color_matching(to_config_group(tgt));
+	color_matching_desc->refcnt--;
+
+	streaming = src->ci_parent->ci_parent;
+	color_matching_desc = uvcg_format_get_default_color_match(streaming);
+
+	fmt = to_uvcg_format(src);
+	fmt->color_matching = color_matching_desc;
+	color_matching_desc->refcnt++;
+
+	mutex_unlock(su_mutex);
+}
+
+static struct configfs_item_operations uvcg_format_item_operations = {
+	.release	= uvcg_config_item_release,
+	.allow_link	= uvcg_format_allow_link,
+	.drop_link	= uvcg_format_drop_link,
+};
+
 static ssize_t uvcg_format_bma_controls_show(struct uvcg_format *f, char *page)
 {
 	struct f_uvc_opts *opts;
@@ -1131,57 +1933,6 @@ static ssize_t uvcg_frame_dw_frame_interval_show(struct config_item *item,
 	return result;
 }
 
-static inline int __uvcg_count_frm_intrv(char *buf, void *priv)
-{
-	++*((int *)priv);
-	return 0;
-}
-
-static inline int __uvcg_fill_frm_intrv(char *buf, void *priv)
-{
-	u32 num, **interv;
-	int ret;
-
-	ret = kstrtou32(buf, 0, &num);
-	if (ret)
-		return ret;
-
-	interv = priv;
-	**interv = num;
-	++*interv;
-
-	return 0;
-}
-
-static int __uvcg_iter_frm_intrv(const char *page, size_t len,
-				 int (*fun)(char *, void *), void *priv)
-{
-	/* sign, base 2 representation, newline, terminator */
-	char buf[1 + sizeof(u32) * 8 + 1 + 1];
-	const char *pg = page;
-	int i, ret;
-
-	if (!fun)
-		return -EINVAL;
-
-	while (pg - page < len) {
-		i = 0;
-		while (i < sizeof(buf) && (pg - page < len) &&
-				*pg != '\0' && *pg != '\n')
-			buf[i++] = *pg++;
-		if (i == sizeof(buf))
-			return -EINVAL;
-		while ((pg - page < len) && (*pg == '\0' || *pg == '\n'))
-			++pg;
-		buf[i] = '\0';
-		ret = fun(buf, priv);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}
-
 static ssize_t uvcg_frame_dw_frame_interval_store(struct config_item *item,
 						  const char *page, size_t len)
 {
@@ -1205,7 +1956,7 @@ static ssize_t uvcg_frame_dw_frame_interval_store(struct config_item *item,
 		goto end;
 	}
 
-	ret = __uvcg_iter_frm_intrv(page, len, __uvcg_count_frm_intrv, &n);
+	ret = __uvcg_iter_item_entries(page, len, __uvcg_count_item_entries, &n, sizeof(u32));
 	if (ret)
 		goto end;
 
@@ -1215,7 +1966,7 @@ static ssize_t uvcg_frame_dw_frame_interval_store(struct config_item *item,
 		goto end;
 	}
 
-	ret = __uvcg_iter_frm_intrv(page, len, __uvcg_fill_frm_intrv, &tmp);
+	ret = __uvcg_iter_item_entries(page, len, __uvcg_fill_item_entries, &tmp, sizeof(u32));
 	if (ret) {
 		kfree(frm_intrv);
 		goto end;
@@ -1547,7 +2298,7 @@ static struct configfs_attribute *uvcg_uncompressed_attrs[] = {
 };
 
 static const struct config_item_type uvcg_uncompressed_type = {
-	.ct_item_ops	= &uvcg_config_item_ops,
+	.ct_item_ops	= &uvcg_format_item_operations,
 	.ct_group_ops	= &uvcg_uncompressed_group_ops,
 	.ct_attrs	= uvcg_uncompressed_attrs,
 	.ct_owner	= THIS_MODULE,
@@ -1560,8 +2311,15 @@ static struct config_group *uvcg_uncompressed_make(struct config_group *group,
 		'Y',  'U',  'Y',  '2', 0x00, 0x00, 0x10, 0x00,
 		 0x80, 0x00, 0x00, 0xaa, 0x00, 0x38, 0x9b, 0x71
 	};
+	struct uvcg_color_matching *color_match;
+	struct config_item *streaming;
 	struct uvcg_uncompressed *h;
 
+	streaming = group->cg_item.ci_parent;
+	color_match = uvcg_format_get_default_color_match(streaming);
+	if (!color_match)
+		return ERR_PTR(-EINVAL);
+
 	h = kzalloc(sizeof(*h), GFP_KERNEL);
 	if (!h)
 		return ERR_PTR(-ENOMEM);
@@ -1579,6 +2337,8 @@ static struct config_group *uvcg_uncompressed_make(struct config_group *group,
 
 	INIT_LIST_HEAD(&h->fmt.frames);
 	h->fmt.type = UVCG_UNCOMPRESSED;
+	h->fmt.color_matching = color_match;
+	color_match->refcnt++;
 	config_group_init_type_name(&h->fmt.group, name,
 				    &uvcg_uncompressed_type);
 
@@ -1734,7 +2494,7 @@ static struct configfs_attribute *uvcg_mjpeg_attrs[] = {
 };
 
 static const struct config_item_type uvcg_mjpeg_type = {
-	.ct_item_ops	= &uvcg_config_item_ops,
+	.ct_item_ops	= &uvcg_format_item_operations,
 	.ct_group_ops	= &uvcg_mjpeg_group_ops,
 	.ct_attrs	= uvcg_mjpeg_attrs,
 	.ct_owner	= THIS_MODULE,
@@ -1743,8 +2503,15 @@ static const struct config_item_type uvcg_mjpeg_type = {
 static struct config_group *uvcg_mjpeg_make(struct config_group *group,
 						   const char *name)
 {
+	struct uvcg_color_matching *color_match;
+	struct config_item *streaming;
 	struct uvcg_mjpeg *h;
 
+	streaming = group->cg_item.ci_parent;
+	color_match = uvcg_format_get_default_color_match(streaming);
+	if (!color_match)
+		return ERR_PTR(-EINVAL);
+
 	h = kzalloc(sizeof(*h), GFP_KERNEL);
 	if (!h)
 		return ERR_PTR(-ENOMEM);
@@ -1760,6 +2527,8 @@ static struct config_group *uvcg_mjpeg_make(struct config_group *group,
 
 	INIT_LIST_HEAD(&h->fmt.frames);
 	h->fmt.type = UVCG_MJPEG;
+	h->fmt.color_matching = color_match;
+	color_match->refcnt++;
 	config_group_init_type_name(&h->fmt.group, name,
 				    &uvcg_mjpeg_type);
 
@@ -1783,70 +2552,159 @@ static const struct uvcg_config_group_type uvcg_mjpeg_grp_type = {
  * streaming/color_matching/default
  */
 
-#define UVCG_DEFAULT_COLOR_MATCHING_ATTR(cname, aname, bits)		\
-static ssize_t uvcg_default_color_matching_##cname##_show(		\
+#define UVCG_COLOR_MATCHING_ATTR(cname, aname, bits)			\
+static ssize_t uvcg_color_matching_##cname##_show(			\
 	struct config_item *item, char *page)				\
 {									\
 	struct config_group *group = to_config_group(item);		\
+	struct uvcg_color_matching *color_match =			\
+		to_uvcg_color_matching(group);				\
 	struct f_uvc_opts *opts;					\
 	struct config_item *opts_item;					\
 	struct mutex *su_mutex = &group->cg_subsys->su_mutex;		\
-	struct uvc_color_matching_descriptor *cd;			\
 	int result;							\
 									\
 	mutex_lock(su_mutex); /* for navigating configfs hierarchy */	\
 									\
 	opts_item = group->cg_item.ci_parent->ci_parent->ci_parent;	\
 	opts = to_f_uvc_opts(opts_item);				\
-	cd = &opts->uvc_color_matching;					\
 									\
 	mutex_lock(&opts->lock);					\
-	result = sprintf(page, "%u\n", le##bits##_to_cpu(cd->aname));	\
+	result = sprintf(page, "%u\n",					\
+			 le##bits##_to_cpu(color_match->desc.aname));	\
 	mutex_unlock(&opts->lock);					\
 									\
 	mutex_unlock(su_mutex);						\
 	return result;							\
 }									\
 									\
-UVC_ATTR_RO(uvcg_default_color_matching_, cname, aname)
+static ssize_t uvcg_color_matching_##cname##_store(			\
+	struct config_item *item, const char *page, size_t len)		\
+{									\
+	struct config_group *group = to_config_group(item);		\
+	struct mutex *su_mutex = &group->cg_subsys->su_mutex;		\
+	struct uvcg_color_matching *color_match =			\
+		to_uvcg_color_matching(group);				\
+	struct f_uvc_opts *opts;					\
+	struct config_item *opts_item;					\
+	int ret;							\
+	u##bits num;							\
+									\
+	ret = kstrtou##bits(page, 0, &num);				\
+	if (ret)							\
+		return ret;						\
+									\
+	mutex_lock(su_mutex); /* for navigating configfs hierarchy */	\
+									\
+	if (color_match->refcnt) {					\
+		ret = -EBUSY;						\
+		goto unlock_su;						\
+	}								\
+									\
+	opts_item = group->cg_item.ci_parent->ci_parent->ci_parent;	\
+	opts = to_f_uvc_opts(opts_item);				\
+									\
+	mutex_lock(&opts->lock);					\
+									\
+	color_match->desc.aname = num;					\
+	ret = len;							\
+									\
+	mutex_unlock(&opts->lock);					\
+unlock_su:								\
+	mutex_unlock(su_mutex);						\
+									\
+	return ret;							\
+}									\
+UVC_ATTR(uvcg_color_matching_, cname, aname)
 
-UVCG_DEFAULT_COLOR_MATCHING_ATTR(b_color_primaries, bColorPrimaries, 8);
-UVCG_DEFAULT_COLOR_MATCHING_ATTR(b_transfer_characteristics,
-				 bTransferCharacteristics, 8);
-UVCG_DEFAULT_COLOR_MATCHING_ATTR(b_matrix_coefficients, bMatrixCoefficients, 8);
+UVCG_COLOR_MATCHING_ATTR(b_color_primaries, bColorPrimaries, 8);
+UVCG_COLOR_MATCHING_ATTR(b_transfer_characteristics, bTransferCharacteristics, 8);
+UVCG_COLOR_MATCHING_ATTR(b_matrix_coefficients, bMatrixCoefficients, 8);
 
-#undef UVCG_DEFAULT_COLOR_MATCHING_ATTR
+#undef UVCG_COLOR_MATCHING_ATTR
 
-static struct configfs_attribute *uvcg_default_color_matching_attrs[] = {
-	&uvcg_default_color_matching_attr_b_color_primaries,
-	&uvcg_default_color_matching_attr_b_transfer_characteristics,
-	&uvcg_default_color_matching_attr_b_matrix_coefficients,
+static struct configfs_attribute *uvcg_color_matching_attrs[] = {
+	&uvcg_color_matching_attr_b_color_primaries,
+	&uvcg_color_matching_attr_b_transfer_characteristics,
+	&uvcg_color_matching_attr_b_matrix_coefficients,
 	NULL,
 };
 
-static const struct uvcg_config_group_type uvcg_default_color_matching_type = {
-	.type = {
-		.ct_item_ops	= &uvcg_config_item_ops,
-		.ct_attrs	= uvcg_default_color_matching_attrs,
-		.ct_owner	= THIS_MODULE,
-	},
-	.name = "default",
+static void uvcg_color_matching_release(struct config_item *item)
+{
+	struct uvcg_color_matching *color_match =
+		to_uvcg_color_matching(to_config_group(item));
+
+	kfree(color_match);
+}
+
+static struct configfs_item_operations uvcg_color_matching_item_ops = {
+	.release	= uvcg_color_matching_release,
+};
+
+static const struct config_item_type uvcg_color_matching_type = {
+	.ct_item_ops	= &uvcg_color_matching_item_ops,
+	.ct_attrs	= uvcg_color_matching_attrs,
+	.ct_owner	= THIS_MODULE,
 };
 
 /* -----------------------------------------------------------------------------
  * streaming/color_matching
  */
 
+static struct config_group *uvcg_color_matching_make(struct config_group *group,
+						     const char *name)
+{
+	struct uvcg_color_matching *color_match;
+
+	color_match = kzalloc(sizeof(*color_match), GFP_KERNEL);
+	if (!color_match)
+		return ERR_PTR(-ENOMEM);
+
+	color_match->desc.bLength = UVC_DT_COLOR_MATCHING_SIZE;
+	color_match->desc.bDescriptorType = USB_DT_CS_INTERFACE;
+	color_match->desc.bDescriptorSubType = UVC_VS_COLORFORMAT;
+
+	config_group_init_type_name(&color_match->group, name,
+				    &uvcg_color_matching_type);
+
+	return &color_match->group;
+}
+
+static struct configfs_group_operations uvcg_color_matching_grp_group_ops = {
+	.make_group	= uvcg_color_matching_make,
+};
+
+static int uvcg_color_matching_create_children(struct config_group *parent)
+{
+	struct uvcg_color_matching *color_match;
+
+	color_match = kzalloc(sizeof(*color_match), GFP_KERNEL);
+	if (!color_match)
+		return -ENOMEM;
+
+	color_match->desc.bLength = UVC_DT_COLOR_MATCHING_SIZE;
+	color_match->desc.bDescriptorType = USB_DT_CS_INTERFACE;
+	color_match->desc.bDescriptorSubType = UVC_VS_COLORFORMAT;
+	color_match->desc.bColorPrimaries = UVC_COLOR_PRIMARIES_BT_709_SRGB;
+	color_match->desc.bTransferCharacteristics = UVC_TRANSFER_CHARACTERISTICS_BT_709;
+	color_match->desc.bMatrixCoefficients = UVC_MATRIX_COEFFICIENTS_SMPTE_170M;
+
+	config_group_init_type_name(&color_match->group, "default",
+				    &uvcg_color_matching_type);
+	configfs_add_default_group(&color_match->group, parent);
+
+	return 0;
+}
+
 static const struct uvcg_config_group_type uvcg_color_matching_grp_type = {
 	.type = {
 		.ct_item_ops	= &uvcg_config_item_ops,
+		.ct_group_ops	= &uvcg_color_matching_grp_group_ops,
 		.ct_owner	= THIS_MODULE,
 	},
 	.name = "color_matching",
-	.children = (const struct uvcg_config_group_type*[]) {
-		&uvcg_default_color_matching_type,
-		NULL,
-	},
+	.create_children = uvcg_color_matching_create_children,
 };
 
 /* -----------------------------------------------------------------------------
@@ -1880,7 +2738,8 @@ static inline struct uvc_descriptor_header
 enum uvcg_strm_type {
 	UVCG_HEADER = 0,
 	UVCG_FORMAT,
-	UVCG_FRAME
+	UVCG_FRAME,
+	UVCG_COLOR_MATCHING,
 };
 
 /*
@@ -1930,6 +2789,11 @@ static int __uvcg_iter_strm_cls(struct uvcg_streaming_header *h,
 			if (ret)
 				return ret;
 		}
+
+		ret = fun(f->fmt->color_matching, priv2, priv3, 0,
+			  UVCG_COLOR_MATCHING);
+		if (ret)
+			return ret;
 	}
 
 	return ret;
@@ -1985,6 +2849,12 @@ static int __uvcg_cnt_strm(void *priv1, void *priv2, void *priv3, int n,
 		*size += frm->frame.b_frame_interval_type * sz;
 	}
 	break;
+	case UVCG_COLOR_MATCHING: {
+		struct uvcg_color_matching *color_match = priv1;
+
+		*size += sizeof(color_match->desc);
+	}
+	break;
 	}
 
 	++*count;
@@ -2070,6 +2940,13 @@ static int __uvcg_fill_strm(void *priv1, void *priv2, void *priv3, int n,
 				frm->frame.b_frame_interval_type);
 	}
 	break;
+	case UVCG_COLOR_MATCHING: {
+		struct uvcg_color_matching *color_match = priv1;
+
+		memcpy(*dest, &color_match->desc, sizeof(color_match->desc));
+		*dest += sizeof(color_match->desc);
+	}
+	break;
 	}
 
 	return 0;
@@ -2109,7 +2986,7 @@ static int uvcg_streaming_class_allow_link(struct config_item *src,
 	if (ret)
 		goto unlock;
 
-	count += 2; /* color_matching, NULL */
+	count += 1; /* NULL */
 	*class_array = kcalloc(count, sizeof(void *), GFP_KERNEL);
 	if (!*class_array) {
 		ret = -ENOMEM;
@@ -2136,7 +3013,6 @@ static int uvcg_streaming_class_allow_link(struct config_item *src,
 		kfree(data_save);
 		goto unlock;
 	}
-	*cl_arr = (struct uvc_descriptor_header *)&opts->uvc_color_matching;
 
 	++target_hdr->linked;
 	ret = 0;
@@ -2298,8 +3174,68 @@ static void uvc_func_item_release(struct config_item *item)
 	usb_put_function_instance(&opts->func_inst);
 }
 
+static int uvc_func_allow_link(struct config_item *src, struct config_item *tgt)
+{
+	struct mutex *su_mutex = &src->ci_group->cg_subsys->su_mutex;
+	struct gadget_string *string;
+	struct config_item *strings;
+	struct f_uvc_opts *opts;
+	int ret = 0;
+
+	mutex_lock(su_mutex); /* for navigating configfs hierarchy */
+
+	/* Validate that the target is an entry in strings/<langid> */
+	strings = config_group_find_item(to_config_group(src->ci_parent->ci_parent),
+					 "strings");
+	if (!strings || tgt->ci_parent->ci_parent != strings) {
+		ret = -EINVAL;
+		goto put_strings;
+	}
+
+	string = to_gadget_string(tgt);
+
+	opts = to_f_uvc_opts(src);
+	mutex_lock(&opts->lock);
+
+	if (!strcmp(tgt->ci_name, "iad_desc"))
+		opts->iad_index = string->usb_string.id;
+	else if (!strcmp(tgt->ci_name, "vs0_desc"))
+		opts->vs0_index = string->usb_string.id;
+	else if (!strcmp(tgt->ci_name, "vs1_desc"))
+		opts->vs1_index = string->usb_string.id;
+	else
+		ret = -EINVAL;
+
+	mutex_unlock(&opts->lock);
+
+put_strings:
+	config_item_put(strings);
+	mutex_unlock(su_mutex);
+
+	return ret;
+}
+
+static void uvc_func_drop_link(struct config_item *src, struct config_item *tgt)
+{
+	struct f_uvc_opts *opts;
+
+	opts = to_f_uvc_opts(src);
+	mutex_lock(&opts->lock);
+
+	if (!strcmp(tgt->ci_name, "iad_desc"))
+		opts->iad_index = 0;
+	else if (!strcmp(tgt->ci_name, "vs0_desc"))
+		opts->vs0_index = 0;
+	else if (!strcmp(tgt->ci_name, "vs1_desc"))
+		opts->vs1_index = 0;
+
+	mutex_unlock(&opts->lock);
+}
+
 static struct configfs_item_operations uvc_func_item_ops = {
 	.release	= uvc_func_item_release,
+	.allow_link	= uvc_func_allow_link,
+	.drop_link	= uvc_func_drop_link,
 };
 
 #define UVCG_OPTS_ATTR(cname, aname, limit)				\
diff --git a/drivers/usb/gadget/function/uvc_configfs.h b/drivers/usb/gadget/function/uvc_configfs.h
index ad2ec8c4c78c..c6a690158138 100644
--- a/drivers/usb/gadget/function/uvc_configfs.h
+++ b/drivers/usb/gadget/function/uvc_configfs.h
@@ -37,18 +37,28 @@ static inline struct uvcg_control_header *to_uvcg_control_header(struct config_i
 	return container_of(item, struct uvcg_control_header, item);
 }
 
+struct uvcg_color_matching {
+	struct config_group group;
+	struct uvc_color_matching_descriptor desc;
+	unsigned int refcnt;
+};
+
+#define to_uvcg_color_matching(group_ptr) \
+container_of(group_ptr, struct uvcg_color_matching, group)
+
 enum uvcg_format_type {
 	UVCG_UNCOMPRESSED = 0,
 	UVCG_MJPEG,
 };
 
 struct uvcg_format {
-	struct config_group	group;
-	enum uvcg_format_type	type;
-	unsigned		linked;
-	struct list_head	frames;
-	unsigned		num_frames;
-	__u8			bmaControls[UVCG_STREAMING_CONTROL_SIZE];
+	struct config_group		group;
+	enum uvcg_format_type		type;
+	unsigned			linked;
+	struct list_head		frames;
+	unsigned			num_frames;
+	__u8				bmaControls[UVCG_STREAMING_CONTROL_SIZE];
+	struct uvcg_color_matching	*color_matching;
 };
 
 struct uvcg_format_ptr {
@@ -132,6 +142,36 @@ static inline struct uvcg_mjpeg *to_uvcg_mjpeg(struct config_item *item)
 	return container_of(to_uvcg_format(item), struct uvcg_mjpeg, fmt);
 }
 
+/* -----------------------------------------------------------------------------
+ * control/extensions/<NAME>
+ */
+
+struct uvcg_extension_unit_descriptor {
+	u8 bLength;
+	u8 bDescriptorType;
+	u8 bDescriptorSubType;
+	u8 bUnitID;
+	u8 guidExtensionCode[16];
+	u8 bNumControls;
+	u8 bNrInPins;
+	u8 *baSourceID;
+	u8 bControlSize;
+	u8 *bmControls;
+	u8 iExtension;
+} __packed;
+
+struct uvcg_extension {
+	struct config_item item;
+	struct list_head list;
+	u8 string_descriptor_index;
+	struct uvcg_extension_unit_descriptor desc;
+};
+
+static inline struct uvcg_extension *to_uvcg_extension(struct config_item *item)
+{
+	return container_of(item, struct uvcg_extension, item);
+}
+
 int uvcg_attach_configfs(struct f_uvc_opts *opts);
 
 #endif /* UVC_CONFIGFS_H */
diff --git a/drivers/usb/gadget/function/uvc_v4l2.c b/drivers/usb/gadget/function/uvc_v4l2.c
index a189b08bba80..3f0a9795c0d4 100644
--- a/drivers/usb/gadget/function/uvc_v4l2.c
+++ b/drivers/usb/gadget/function/uvc_v4l2.c
@@ -11,6 +11,7 @@
 #include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/usb/g_uvc.h>
+#include <linux/usb/uvc.h>
 #include <linux/videodev2.h>
 #include <linux/vmalloc.h>
 #include <linux/wait.h>
@@ -18,7 +19,6 @@
 #include <media/v4l2-dev.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-ioctl.h>
-#include <media/v4l2-uvc.h>
 
 #include "f_uvc.h"
 #include "uvc.h"
@@ -27,10 +27,10 @@
 #include "uvc_v4l2.h"
 #include "uvc_configfs.h"
 
-static struct uvc_format_desc *to_uvc_format(struct uvcg_format *uformat)
+static const struct uvc_format_desc *to_uvc_format(struct uvcg_format *uformat)
 {
 	char guid[16] = UVC_GUID_FORMAT_MJPEG;
-	struct uvc_format_desc *format;
+	const struct uvc_format_desc *format;
 	struct uvcg_uncompressed *unc;
 
 	if (uformat->type == UVCG_UNCOMPRESSED) {
@@ -119,7 +119,7 @@ static struct uvcg_format *find_format_by_pix(struct uvc_device *uvc,
 	struct uvcg_format *uformat = NULL;
 
 	list_for_each_entry(format, &uvc->header->formats, entry) {
-		struct uvc_format_desc *fmtdesc = to_uvc_format(format->fmt);
+		const struct uvc_format_desc *fmtdesc = to_uvc_format(format->fmt);
 
 		if (fmtdesc->fcc == pixelformat) {
 			uformat = format->fmt;
@@ -364,7 +364,7 @@ uvc_v4l2_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct uvc_device *uvc = video_get_drvdata(vdev);
-	struct uvc_format_desc *fmtdesc;
+	const struct uvc_format_desc *fmtdesc;
 	struct uvcg_format *uformat;
 
 	if (f->index >= uvc->header->num_fmt)
@@ -374,15 +374,9 @@ uvc_v4l2_enum_format(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 	if (!uformat)
 		return -EINVAL;
 
-	if (uformat->type != UVCG_UNCOMPRESSED)
-		f->flags |= V4L2_FMT_FLAG_COMPRESSED;
-
 	fmtdesc = to_uvc_format(uformat);
 	f->pixelformat = fmtdesc->fcc;
 
-	strscpy(f->description, fmtdesc->name, sizeof(f->description));
-	f->description[strlen(fmtdesc->name) - 1] = 0;
-
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/legacy/hid.c b/drivers/usb/gadget/legacy/hid.c
index 1187ee4f316a..133daf88162e 100644
--- a/drivers/usb/gadget/legacy/hid.c
+++ b/drivers/usb/gadget/legacy/hid.c
@@ -133,14 +133,11 @@ static struct usb_configuration config_driver = {
 static int hid_bind(struct usb_composite_dev *cdev)
 {
 	struct usb_gadget *gadget = cdev->gadget;
-	struct list_head *tmp;
 	struct hidg_func_node *n = NULL, *m, *iter_n;
 	struct f_hid_opts *hid_opts;
-	int status, funcs = 0;
-
-	list_for_each(tmp, &hidg_func_list)
-		funcs++;
+	int status, funcs;
 
+	funcs = list_count_nodes(&hidg_func_list);
 	if (!funcs)
 		return -ENODEV;
 
diff --git a/drivers/usb/gadget/udc/Kconfig b/drivers/usb/gadget/udc/Kconfig
index 511ab57cdc81..83cae6bb12eb 100644
--- a/drivers/usb/gadget/udc/Kconfig
+++ b/drivers/usb/gadget/udc/Kconfig
@@ -179,9 +179,20 @@ config USB_RENESAS_USBHS_UDC
 	   dynamically linked module called "renesas_usbhs" and force all
 	   gadget drivers to also be dynamically linked.
 
+config USB_RZV2M_USB3DRD
+	tristate 'Renesas USB3.1 DRD controller'
+	depends on ARCH_R9A09G011 || COMPILE_TEST
+	help
+	   Renesas USB3.1 DRD controller is a USB DRD controller
+	   that supports both host and device switching.
+
+	   Say "y" to link the driver statically, or "m" to build a
+	   dynamically linked module called "rzv2m_usb3drd".
+
 config USB_RENESAS_USB3
 	tristate 'Renesas USB3.0 Peripheral controller'
 	depends on ARCH_RENESAS || COMPILE_TEST
+	depends on USB_RZV2M_USB3DRD || !USB_RZV2M_USB3DRD
 	depends on EXTCON
 	select USB_ROLE_SWITCH
 	help
@@ -192,6 +203,17 @@ config USB_RENESAS_USB3
 	   dynamically linked module called "renesas_usb3" and force all
 	   gadget drivers to also be dynamically linked.
 
+config USB_RENESAS_USBF
+	tristate 'Renesas USB Function controller'
+	depends on ARCH_RENESAS || COMPILE_TEST
+	help
+	   Renesas USB Function controller is a USB peripheral controller
+	   available on RZ/N1 Renesas SoCs.
+
+	   Say "y" to link the driver statically, or "m" to build a
+	   dynamically linked module called "renesas_usbf" and force all
+	   gadget drivers to also be dynamically linked.
+
 config USB_PXA27X
 	tristate "PXA 27x"
 	depends on HAS_IOMEM
diff --git a/drivers/usb/gadget/udc/Makefile b/drivers/usb/gadget/udc/Makefile
index 239ea22bdfd9..ee569f63c74a 100644
--- a/drivers/usb/gadget/udc/Makefile
+++ b/drivers/usb/gadget/udc/Makefile
@@ -26,6 +26,8 @@ obj-$(CONFIG_USB_TEGRA_XUDC)	+= tegra-xudc.o
 obj-$(CONFIG_USB_M66592)	+= m66592-udc.o
 obj-$(CONFIG_USB_R8A66597)	+= r8a66597-udc.o
 obj-$(CONFIG_USB_RENESAS_USB3)	+= renesas_usb3.o
+obj-$(CONFIG_USB_RZV2M_USB3DRD)	+= rzv2m_usb3drd.o
+obj-$(CONFIG_USB_RENESAS_USBF)	+= renesas_usbf.o
 obj-$(CONFIG_USB_FSL_QE)	+= fsl_qe_udc.o
 obj-$(CONFIG_USB_LPC32XX)	+= lpc32xx_udc.o
 obj-$(CONFIG_USB_EG20T)		+= pch_udc.o
diff --git a/drivers/usb/gadget/udc/bcm63xx_udc.c b/drivers/usb/gadget/udc/bcm63xx_udc.c
index d04d72f5816e..a3055dd4acfb 100644
--- a/drivers/usb/gadget/udc/bcm63xx_udc.c
+++ b/drivers/usb/gadget/udc/bcm63xx_udc.c
@@ -2171,7 +2171,6 @@ static int bcm63xx_iudma_dbg_show(struct seq_file *s, void *p)
 
 	for (ch_idx = 0; ch_idx < BCM63XX_NUM_IUDMA; ch_idx++) {
 		struct iudma_ch *iudma = &udc->iudma[ch_idx];
-		struct list_head *pos;
 
 		seq_printf(s, "IUDMA channel %d -- ", ch_idx);
 		switch (iudma_defaults[ch_idx].ep_type) {
@@ -2204,14 +2203,10 @@ static int bcm63xx_iudma_dbg_show(struct seq_file *s, void *p)
 		seq_printf(s, "  desc: %d/%d used", iudma->n_bds_used,
 			   iudma->n_bds);
 
-		if (iudma->bep) {
-			i = 0;
-			list_for_each(pos, &iudma->bep->queue)
-				i++;
-			seq_printf(s, "; %d queued\n", i);
-		} else {
+		if (iudma->bep)
+			seq_printf(s, "; %zu queued\n", list_count_nodes(&iudma->bep->queue));
+		else
 			seq_printf(s, "\n");
-		}
 
 		for (i = 0; i < iudma->n_bds; i++) {
 			struct bcm_enet_desc *d = &iudma->bd_ring[i];
@@ -2258,7 +2253,7 @@ static void bcm63xx_udc_init_debugfs(struct bcm63xx_udc *udc)
  */
 static void bcm63xx_udc_cleanup_debugfs(struct bcm63xx_udc *udc)
 {
-	debugfs_remove(debugfs_lookup(udc->gadget.name, usb_debug_root));
+	debugfs_lookup_and_remove(udc->gadget.name, usb_debug_root);
 }
 
 /***********************************************************************
diff --git a/drivers/usb/gadget/udc/fusb300_udc.c b/drivers/usb/gadget/udc/fusb300_udc.c
index 5954800d652c..08ba9c8c1e67 100644
--- a/drivers/usb/gadget/udc/fusb300_udc.c
+++ b/drivers/usb/gadget/udc/fusb300_udc.c
@@ -1346,6 +1346,7 @@ static int fusb300_remove(struct platform_device *pdev)
 	usb_del_gadget_udc(&fusb300->gadget);
 	iounmap(fusb300->reg);
 	free_irq(platform_get_irq(pdev, 0), fusb300);
+	free_irq(platform_get_irq(pdev, 1), fusb300);
 
 	fusb300_free_request(&fusb300->ep[0]->ep, fusb300->ep0_req);
 	for (i = 0; i < FUSB300_MAX_NUM_EP; i++)
@@ -1431,7 +1432,7 @@ static int fusb300_probe(struct platform_device *pdev)
 			IRQF_SHARED, udc_name, fusb300);
 	if (ret < 0) {
 		pr_err("request_irq1 error (%d)\n", ret);
-		goto clean_up;
+		goto err_request_irq1;
 	}
 
 	INIT_LIST_HEAD(&fusb300->gadget.ep_list);
@@ -1470,7 +1471,7 @@ static int fusb300_probe(struct platform_device *pdev)
 				GFP_KERNEL);
 	if (fusb300->ep0_req == NULL) {
 		ret = -ENOMEM;
-		goto clean_up3;
+		goto err_alloc_request;
 	}
 
 	init_controller(fusb300);
@@ -1485,7 +1486,10 @@ static int fusb300_probe(struct platform_device *pdev)
 err_add_udc:
 	fusb300_free_request(&fusb300->ep[0]->ep, fusb300->ep0_req);
 
-clean_up3:
+err_alloc_request:
+	free_irq(ires1->start, fusb300);
+
+err_request_irq1:
 	free_irq(ires->start, fusb300);
 
 clean_up:
diff --git a/drivers/usb/gadget/udc/gr_udc.c b/drivers/usb/gadget/udc/gr_udc.c
index 85cdc0af3bf9..09762559912d 100644
--- a/drivers/usb/gadget/udc/gr_udc.c
+++ b/drivers/usb/gadget/udc/gr_udc.c
@@ -215,7 +215,7 @@ static void gr_dfs_create(struct gr_udc *dev)
 
 static void gr_dfs_delete(struct gr_udc *dev)
 {
-	debugfs_remove(debugfs_lookup(dev_name(dev->dev), usb_debug_root));
+	debugfs_lookup_and_remove(dev_name(dev->dev), usb_debug_root);
 }
 
 #else /* !CONFIG_USB_GADGET_DEBUG_FS */
diff --git a/drivers/usb/gadget/udc/lpc32xx_udc.c b/drivers/usb/gadget/udc/lpc32xx_udc.c
index cea10cdb83ae..fe62db32dd0e 100644
--- a/drivers/usb/gadget/udc/lpc32xx_udc.c
+++ b/drivers/usb/gadget/udc/lpc32xx_udc.c
@@ -532,7 +532,7 @@ static void create_debug_file(struct lpc32xx_udc *udc)
 
 static void remove_debug_file(struct lpc32xx_udc *udc)
 {
-	debugfs_remove(debugfs_lookup(debug_filename, NULL));
+	debugfs_lookup_and_remove(debug_filename, NULL);
 }
 
 #else
diff --git a/drivers/usb/gadget/udc/pxa25x_udc.c b/drivers/usb/gadget/udc/pxa25x_udc.c
index e19b84a46a1e..df0551ecc810 100644
--- a/drivers/usb/gadget/udc/pxa25x_udc.c
+++ b/drivers/usb/gadget/udc/pxa25x_udc.c
@@ -1340,7 +1340,7 @@ DEFINE_SHOW_ATTRIBUTE(udc_debug);
 		debugfs_create_file(dev->gadget.name, \
 			S_IRUGO, NULL, dev, &udc_debug_fops); \
 	} while (0)
-#define remove_debug_files(dev) debugfs_remove(debugfs_lookup(dev->gadget.name, NULL))
+#define remove_debug_files(dev) debugfs_lookup_and_remove(dev->gadget.name, NULL)
 
 #else	/* !CONFIG_USB_GADGET_DEBUG_FILES */
 
diff --git a/drivers/usb/gadget/udc/pxa27x_udc.c b/drivers/usb/gadget/udc/pxa27x_udc.c
index ac980d6a4740..0ecdfd2ba9e9 100644
--- a/drivers/usb/gadget/udc/pxa27x_udc.c
+++ b/drivers/usb/gadget/udc/pxa27x_udc.c
@@ -215,7 +215,7 @@ static void pxa_init_debugfs(struct pxa_udc *udc)
 
 static void pxa_cleanup_debugfs(struct pxa_udc *udc)
 {
-	debugfs_remove(debugfs_lookup(udc->gadget.name, usb_debug_root));
+	debugfs_lookup_and_remove(udc->gadget.name, usb_debug_root);
 }
 
 #else
diff --git a/drivers/usb/gadget/udc/renesas_usb3.c b/drivers/usb/gadget/udc/renesas_usb3.c
index 615ba0a6fbee..bee6bceafc4f 100644
--- a/drivers/usb/gadget/udc/renesas_usb3.c
+++ b/drivers/usb/gadget/udc/renesas_usb3.c
@@ -7,6 +7,7 @@
 
 #include <linux/debugfs.h>
 #include <linux/delay.h>
+#include <linux/device.h>
 #include <linux/dma-mapping.h>
 #include <linux/err.h>
 #include <linux/extcon-provider.h>
@@ -27,6 +28,7 @@
 #include <linux/usb/gadget.h>
 #include <linux/usb/of.h>
 #include <linux/usb/role.h>
+#include <linux/usb/rzv2m_usb3drd.h>
 
 /* register definitions */
 #define USB3_AXI_INT_STA	0x008
@@ -334,7 +336,7 @@ struct renesas_usb3_priv {
 
 struct renesas_usb3 {
 	void __iomem *reg;
-	struct reset_control *drd_rstc;
+	void __iomem *drd_reg;
 	struct reset_control *usbp_rstc;
 
 	struct usb_gadget gadget;
@@ -426,6 +428,46 @@ static void usb3_clear_bit(struct renesas_usb3 *usb3, u32 bits, u32 offs)
 	usb3_write(usb3, val, offs);
 }
 
+static void usb3_drd_write(struct renesas_usb3 *usb3, u32 data, u32 offs)
+{
+	void __iomem *reg;
+
+	if (usb3->is_rzv2m)
+		reg = usb3->drd_reg + offs - USB3_DRD_CON(usb3);
+	else
+		reg = usb3->reg + offs;
+
+	iowrite32(data, reg);
+}
+
+static u32 usb3_drd_read(struct renesas_usb3 *usb3, u32 offs)
+{
+	void __iomem *reg;
+
+	if (usb3->is_rzv2m)
+		reg = usb3->drd_reg + offs - USB3_DRD_CON(usb3);
+	else
+		reg = usb3->reg + offs;
+
+	return ioread32(reg);
+}
+
+static void usb3_drd_set_bit(struct renesas_usb3 *usb3, u32 bits, u32 offs)
+{
+	u32 val = usb3_drd_read(usb3, offs);
+
+	val |= bits;
+	usb3_drd_write(usb3, val, offs);
+}
+
+static void usb3_drd_clear_bit(struct renesas_usb3 *usb3, u32 bits, u32 offs)
+{
+	u32 val = usb3_drd_read(usb3, offs);
+
+	val &= ~bits;
+	usb3_drd_write(usb3, val, offs);
+}
+
 static int usb3_wait(struct renesas_usb3 *usb3, u32 reg, u32 mask,
 		     u32 expected)
 {
@@ -474,7 +516,7 @@ static void usb3_disable_pipe_irq(struct renesas_usb3 *usb3, int num)
 
 static bool usb3_is_host(struct renesas_usb3 *usb3)
 {
-	return !(usb3_read(usb3, USB3_DRD_CON(usb3)) & DRD_CON_PERI_CON);
+	return !(usb3_drd_read(usb3, USB3_DRD_CON(usb3)) & DRD_CON_PERI_CON);
 }
 
 static void usb3_init_axi_bridge(struct renesas_usb3 *usb3)
@@ -683,18 +725,18 @@ static void usb3_set_mode(struct renesas_usb3 *usb3, bool host)
 {
 	if (usb3->is_rzv2m) {
 		if (host) {
-			usb3_set_bit(usb3, DRD_CON_PERI_RST, USB3_DRD_CON(usb3));
-			usb3_clear_bit(usb3, DRD_CON_HOST_RST, USB3_DRD_CON(usb3));
+			usb3_drd_set_bit(usb3, DRD_CON_PERI_RST, USB3_DRD_CON(usb3));
+			usb3_drd_clear_bit(usb3, DRD_CON_HOST_RST, USB3_DRD_CON(usb3));
 		} else {
-			usb3_set_bit(usb3, DRD_CON_HOST_RST, USB3_DRD_CON(usb3));
-			usb3_clear_bit(usb3, DRD_CON_PERI_RST, USB3_DRD_CON(usb3));
+			usb3_drd_set_bit(usb3, DRD_CON_HOST_RST, USB3_DRD_CON(usb3));
+			usb3_drd_clear_bit(usb3, DRD_CON_PERI_RST, USB3_DRD_CON(usb3));
 		}
 	}
 
 	if (host)
-		usb3_clear_bit(usb3, DRD_CON_PERI_CON, USB3_DRD_CON(usb3));
+		usb3_drd_clear_bit(usb3, DRD_CON_PERI_CON, USB3_DRD_CON(usb3));
 	else
-		usb3_set_bit(usb3, DRD_CON_PERI_CON, USB3_DRD_CON(usb3));
+		usb3_drd_set_bit(usb3, DRD_CON_PERI_CON, USB3_DRD_CON(usb3));
 }
 
 static void usb3_set_mode_by_role_sw(struct renesas_usb3 *usb3, bool host)
@@ -710,9 +752,9 @@ static void usb3_set_mode_by_role_sw(struct renesas_usb3 *usb3, bool host)
 static void usb3_vbus_out(struct renesas_usb3 *usb3, bool enable)
 {
 	if (enable)
-		usb3_set_bit(usb3, DRD_CON_VBOUT, USB3_DRD_CON(usb3));
+		usb3_drd_set_bit(usb3, DRD_CON_VBOUT, USB3_DRD_CON(usb3));
 	else
-		usb3_clear_bit(usb3, DRD_CON_VBOUT, USB3_DRD_CON(usb3));
+		usb3_drd_clear_bit(usb3, DRD_CON_VBOUT, USB3_DRD_CON(usb3));
 }
 
 static void usb3_mode_config(struct renesas_usb3 *usb3, bool host, bool a_dev)
@@ -733,7 +775,7 @@ static void usb3_mode_config(struct renesas_usb3 *usb3, bool host, bool a_dev)
 
 static bool usb3_is_a_device(struct renesas_usb3 *usb3)
 {
-	return !(usb3_read(usb3, USB3_USB_OTG_STA(usb3)) & USB_OTG_IDMON(usb3));
+	return !(usb3_drd_read(usb3, USB3_USB_OTG_STA(usb3)) & USB_OTG_IDMON(usb3));
 }
 
 static void usb3_check_id(struct renesas_usb3 *usb3)
@@ -756,8 +798,8 @@ static void renesas_usb3_init_controller(struct renesas_usb3 *usb3)
 	usb3_set_bit(usb3, USB_COM_CON_PN_WDATAIF_NL |
 		     USB_COM_CON_PN_RDATAIF_NL | USB_COM_CON_PN_LSTTR_PP,
 		     USB3_USB_COM_CON);
-	usb3_write(usb3, USB_OTG_IDMON(usb3), USB3_USB_OTG_INT_STA(usb3));
-	usb3_write(usb3, USB_OTG_IDMON(usb3), USB3_USB_OTG_INT_ENA(usb3));
+	usb3_drd_write(usb3, USB_OTG_IDMON(usb3), USB3_USB_OTG_INT_STA(usb3));
+	usb3_drd_write(usb3, USB_OTG_IDMON(usb3), USB3_USB_OTG_INT_ENA(usb3));
 
 	usb3_check_id(usb3);
 	usb3_check_vbus(usb3);
@@ -767,7 +809,7 @@ static void renesas_usb3_stop_controller(struct renesas_usb3 *usb3)
 {
 	usb3_disconnect(usb3);
 	usb3_write(usb3, 0, USB3_P0_INT_ENA);
-	usb3_write(usb3, 0, USB3_USB_OTG_INT_ENA(usb3));
+	usb3_drd_write(usb3, 0, USB3_USB_OTG_INT_ENA(usb3));
 	usb3_write(usb3, 0, USB3_USB_INT_ENA_1);
 	usb3_write(usb3, 0, USB3_USB_INT_ENA_2);
 	usb3_write(usb3, 0, USB3_AXI_INT_ENA);
@@ -2024,11 +2066,11 @@ static void usb3_irq_idmon_change(struct renesas_usb3 *usb3)
 
 static void usb3_irq_otg_int(struct renesas_usb3 *usb3)
 {
-	u32 otg_int_sta = usb3_read(usb3, USB3_USB_OTG_INT_STA(usb3));
+	u32 otg_int_sta = usb3_drd_read(usb3, USB3_USB_OTG_INT_STA(usb3));
 
-	otg_int_sta &= usb3_read(usb3, USB3_USB_OTG_INT_ENA(usb3));
+	otg_int_sta &= usb3_drd_read(usb3, USB3_USB_OTG_INT_ENA(usb3));
 	if (otg_int_sta)
-		usb3_write(usb3, otg_int_sta, USB3_USB_OTG_INT_STA(usb3));
+		usb3_drd_write(usb3, otg_int_sta, USB3_USB_OTG_INT_STA(usb3));
 
 	if (otg_int_sta & USB_OTG_IDMON(usb3))
 		usb3_irq_idmon_change(usb3);
@@ -2325,6 +2367,9 @@ static int renesas_usb3_start(struct usb_gadget *gadget,
 
 	usb3 = gadget_to_renesas_usb3(gadget);
 
+	if (usb3->is_rzv2m && usb3_is_a_device(usb3))
+		return -EBUSY;
+
 	/* hook up the driver */
 	usb3->driver = driver;
 
@@ -2333,6 +2378,10 @@ static int renesas_usb3_start(struct usb_gadget *gadget,
 
 	pm_runtime_get_sync(usb3_to_dev(usb3));
 
+	/* Peripheral Reset */
+	if (usb3->is_rzv2m)
+		rzv2m_usb3drd_reset(usb3_to_dev(usb3)->parent, false);
+
 	renesas_usb3_init_controller(usb3);
 
 	return 0;
@@ -2345,8 +2394,10 @@ static int renesas_usb3_stop(struct usb_gadget *gadget)
 	usb3->softconnect = false;
 	usb3->gadget.speed = USB_SPEED_UNKNOWN;
 	usb3->driver = NULL;
-	renesas_usb3_stop_controller(usb3);
+	if (usb3->is_rzv2m)
+		rzv2m_usb3drd_reset(usb3_to_dev(usb3)->parent, false);
 
+	renesas_usb3_stop_controller(usb3);
 	if (usb3->phy)
 		phy_exit(usb3->phy);
 
@@ -2406,18 +2457,29 @@ static void handle_ext_role_switch_states(struct device *dev,
 	switch (role) {
 	case USB_ROLE_NONE:
 		usb3->connection_state = USB_ROLE_NONE;
-		if (cur_role == USB_ROLE_HOST)
+		if (!usb3->is_rzv2m && cur_role == USB_ROLE_HOST)
 			device_release_driver(host);
-		if (usb3->driver)
+		if (usb3->driver) {
+			if (usb3->is_rzv2m)
+				rzv2m_usb3drd_reset(dev->parent, false);
 			usb3_disconnect(usb3);
+		}
 		usb3_vbus_out(usb3, false);
+
+		if (usb3->is_rzv2m) {
+			rzv2m_usb3drd_reset(dev->parent, true);
+			device_release_driver(host);
+		}
 		break;
 	case USB_ROLE_DEVICE:
 		if (usb3->connection_state == USB_ROLE_NONE) {
 			usb3->connection_state = USB_ROLE_DEVICE;
 			usb3_set_mode(usb3, false);
-			if (usb3->driver)
+			if (usb3->driver) {
+				if (usb3->is_rzv2m)
+					renesas_usb3_init_controller(usb3);
 				usb3_connect(usb3);
+			}
 		} else if (cur_role == USB_ROLE_HOST)  {
 			device_release_driver(host);
 			usb3_set_mode(usb3, false);
@@ -2428,8 +2490,11 @@ static void handle_ext_role_switch_states(struct device *dev,
 		break;
 	case USB_ROLE_HOST:
 		if (usb3->connection_state == USB_ROLE_NONE) {
-			if (usb3->driver)
+			if (usb3->driver) {
+				if (usb3->is_rzv2m)
+					rzv2m_usb3drd_reset(dev->parent, false);
 				usb3_disconnect(usb3);
+			}
 
 			usb3->connection_state = USB_ROLE_HOST;
 			usb3_set_mode(usb3, true);
@@ -2600,7 +2665,6 @@ static int renesas_usb3_remove(struct platform_device *pdev)
 
 	usb_del_gadget_udc(&usb3->gadget);
 	reset_control_assert(usb3->usbp_rstc);
-	reset_control_assert(usb3->drd_rstc);
 	renesas_usb3_dma_free_prd(usb3, &pdev->dev);
 
 	__renesas_usb3_ep_free_request(usb3->ep0_req);
@@ -2788,7 +2852,7 @@ static struct usb_role_switch_desc renesas_usb3_role_switch_desc = {
 static int renesas_usb3_probe(struct platform_device *pdev)
 {
 	struct renesas_usb3 *usb3;
-	int irq, drd_irq, ret;
+	int irq, ret;
 	const struct renesas_usb3_priv *priv;
 	const struct soc_device_attribute *attr;
 
@@ -2802,12 +2866,6 @@ static int renesas_usb3_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return irq;
 
-	if (priv->is_rzv2m) {
-		drd_irq = platform_get_irq_byname(pdev, "drd");
-		if (drd_irq < 0)
-			return drd_irq;
-	}
-
 	usb3 = devm_kzalloc(&pdev->dev, sizeof(*usb3), GFP_KERNEL);
 	if (!usb3)
 		return -ENOMEM;
@@ -2836,9 +2894,12 @@ static int renesas_usb3_probe(struct platform_device *pdev)
 		return ret;
 
 	if (usb3->is_rzv2m) {
-		ret = devm_request_irq(&pdev->dev, drd_irq,
+		struct rzv2m_usb3drd *ddata = dev_get_drvdata(pdev->dev.parent);
+
+		usb3->drd_reg = ddata->reg;
+		ret = devm_request_irq(ddata->dev, ddata->drd_irq,
 				       renesas_usb3_otg_irq, 0,
-				       dev_name(&pdev->dev), usb3);
+				       dev_name(ddata->dev), usb3);
 		if (ret < 0)
 			return ret;
 	}
@@ -2873,21 +2934,13 @@ static int renesas_usb3_probe(struct platform_device *pdev)
 		goto err_add_udc;
 	}
 
-	usb3->drd_rstc = devm_reset_control_get_optional_shared(&pdev->dev,
-								"drd_reset");
-	if (IS_ERR(usb3->drd_rstc)) {
-		ret = PTR_ERR(usb3->drd_rstc);
-		goto err_add_udc;
-	}
-
 	usb3->usbp_rstc = devm_reset_control_get_optional_shared(&pdev->dev,
-								 "aresetn_p");
+								 NULL);
 	if (IS_ERR(usb3->usbp_rstc)) {
 		ret = PTR_ERR(usb3->usbp_rstc);
 		goto err_add_udc;
 	}
 
-	reset_control_deassert(usb3->drd_rstc);
 	reset_control_deassert(usb3->usbp_rstc);
 
 	pm_runtime_enable(&pdev->dev);
@@ -2933,7 +2986,6 @@ err_dev_create:
 
 err_reset:
 	reset_control_assert(usb3->usbp_rstc);
-	reset_control_assert(usb3->drd_rstc);
 
 err_add_udc:
 	renesas_usb3_dma_free_prd(usb3, &pdev->dev);
diff --git a/drivers/usb/gadget/udc/renesas_usbf.c b/drivers/usb/gadget/udc/renesas_usbf.c
new file mode 100644
index 000000000000..cb23e62e8a87
--- /dev/null
+++ b/drivers/usb/gadget/udc/renesas_usbf.c
@@ -0,0 +1,3406 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Renesas USBF USB Function driver
+ *
+ * Copyright 2022 Schneider Electric
+ * Author: Herve Codina <herve.codina@bootlin.com>
+ */
+
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/iopoll.h>
+#include <linux/kernel.h>
+#include <linux/kfifo.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/pm_runtime.h>
+#include <linux/types.h>
+#include <linux/usb/composite.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/role.h>
+
+#define USBF_NUM_ENDPOINTS	16
+#define USBF_EP0_MAX_PCKT_SIZE	64
+
+/* EPC registers */
+#define USBF_REG_USB_CONTROL	0x000
+#define     USBF_USB_PUE2		BIT(2)
+#define     USBF_USB_CONNECTB		BIT(3)
+#define     USBF_USB_DEFAULT		BIT(4)
+#define     USBF_USB_CONF		BIT(5)
+#define     USBF_USB_SUSPEND		BIT(6)
+#define     USBF_USB_RSUM_IN		BIT(7)
+#define     USBF_USB_SOF_RCV		BIT(8)
+#define     USBF_USB_FORCEFS		BIT(9)
+#define     USBF_USB_INT_SEL		BIT(10)
+#define     USBF_USB_SOF_CLK_MODE	BIT(11)
+
+#define USBF_REG_USB_STATUS	0x004
+#define     USBF_USB_RSUM_OUT		BIT(1)
+#define     USBF_USB_SPND_OUT		BIT(2)
+#define     USBF_USB_USB_RST		BIT(3)
+#define     USBF_USB_DEFAULT_ST		BIT(4)
+#define     USBF_USB_CONF_ST		BIT(5)
+#define     USBF_USB_SPEED_MODE		BIT(6)
+#define     USBF_USB_SOF_DELAY_STATUS	BIT(31)
+
+#define USBF_REG_USB_ADDRESS	0x008
+#define     USBF_USB_SOF_STATUS		BIT(15)
+#define     USBF_USB_SET_USB_ADDR(_a)	((_a) << 16)
+#define     USBF_USB_GET_FRAME(_r)	((_r) & 0x7FF)
+
+#define USBF_REG_SETUP_DATA0	0x018
+#define USBF_REG_SETUP_DATA1	0x01C
+#define USBF_REG_USB_INT_STA	0x020
+#define     USBF_USB_RSUM_INT		BIT(1)
+#define     USBF_USB_SPND_INT		BIT(2)
+#define     USBF_USB_USB_RST_INT	BIT(3)
+#define     USBF_USB_SOF_INT		BIT(4)
+#define     USBF_USB_SOF_ERROR_INT	BIT(5)
+#define     USBF_USB_SPEED_MODE_INT	BIT(6)
+#define     USBF_USB_EPN_INT(_n)	(BIT(8) << (_n)) /* n=0..15 */
+
+#define USBF_REG_USB_INT_ENA	0x024
+#define     USBF_USB_RSUM_EN		BIT(1)
+#define     USBF_USB_SPND_EN		BIT(2)
+#define     USBF_USB_USB_RST_EN		BIT(3)
+#define     USBF_USB_SOF_EN		BIT(4)
+#define     USBF_USB_SOF_ERROR_EN	BIT(5)
+#define     USBF_USB_SPEED_MODE_EN	BIT(6)
+#define     USBF_USB_EPN_EN(_n)		(BIT(8) << (_n)) /* n=0..15 */
+
+#define USBF_BASE_EP0		0x028
+/* EP0 registers offsets from Base + USBF_BASE_EP0 (EP0 regs area) */
+#define     USBF_REG_EP0_CONTROL	0x00
+#define         USBF_EP0_ONAK			BIT(0)
+#define         USBF_EP0_INAK			BIT(1)
+#define         USBF_EP0_STL			BIT(2)
+#define         USBF_EP0_PERR_NAK_CLR		BIT(3)
+#define         USBF_EP0_INAK_EN		BIT(4)
+#define         USBF_EP0_DW_MASK		(0x3 << 5)
+#define         USBF_EP0_DW(_s)			((_s) << 5)
+#define         USBF_EP0_DEND			BIT(7)
+#define         USBF_EP0_BCLR			BIT(8)
+#define         USBF_EP0_PIDCLR			BIT(9)
+#define         USBF_EP0_AUTO			BIT(16)
+#define         USBF_EP0_OVERSEL		BIT(17)
+#define         USBF_EP0_STGSEL			BIT(18)
+
+#define     USBF_REG_EP0_STATUS		0x04
+#define         USBF_EP0_SETUP_INT		BIT(0)
+#define         USBF_EP0_STG_START_INT		BIT(1)
+#define         USBF_EP0_STG_END_INT		BIT(2)
+#define         USBF_EP0_STALL_INT		BIT(3)
+#define         USBF_EP0_IN_INT			BIT(4)
+#define         USBF_EP0_OUT_INT		BIT(5)
+#define         USBF_EP0_OUT_OR_INT		BIT(6)
+#define         USBF_EP0_OUT_NULL_INT		BIT(7)
+#define         USBF_EP0_IN_EMPTY		BIT(8)
+#define         USBF_EP0_IN_FULL		BIT(9)
+#define         USBF_EP0_IN_DATA		BIT(10)
+#define         USBF_EP0_IN_NAK_INT		BIT(11)
+#define         USBF_EP0_OUT_EMPTY		BIT(12)
+#define         USBF_EP0_OUT_FULL		BIT(13)
+#define         USBF_EP0_OUT_NULL		BIT(14)
+#define         USBF_EP0_OUT_NAK_INT		BIT(15)
+#define         USBF_EP0_PERR_NAK_INT		BIT(16)
+#define         USBF_EP0_PERR_NAK		BIT(17)
+#define         USBF_EP0_PID			BIT(18)
+
+#define     USBF_REG_EP0_INT_ENA	0x08
+#define         USBF_EP0_SETUP_EN		BIT(0)
+#define         USBF_EP0_STG_START_EN		BIT(1)
+#define         USBF_EP0_STG_END_EN		BIT(2)
+#define         USBF_EP0_STALL_EN		BIT(3)
+#define         USBF_EP0_IN_EN			BIT(4)
+#define         USBF_EP0_OUT_EN			BIT(5)
+#define         USBF_EP0_OUT_OR_EN		BIT(6)
+#define         USBF_EP0_OUT_NULL_EN		BIT(7)
+#define         USBF_EP0_IN_NAK_EN		BIT(11)
+#define         USBF_EP0_OUT_NAK_EN		BIT(15)
+#define         USBF_EP0_PERR_NAK_EN		BIT(16)
+
+#define     USBF_REG_EP0_LENGTH		0x0C
+#define         USBF_EP0_LDATA			(0x7FF << 0)
+#define     USBF_REG_EP0_READ		0x10
+#define     USBF_REG_EP0_WRITE		0x14
+
+#define USBF_BASE_EPN(_n)	(0x040 + (_n) * 0x020)
+/* EPn registers offsets from Base + USBF_BASE_EPN(n-1). n=1..15 */
+#define     USBF_REG_EPN_CONTROL	0x000
+#define         USBF_EPN_ONAK			BIT(0)
+#define         USBF_EPN_OSTL			BIT(2)
+#define         USBF_EPN_ISTL			BIT(3)
+#define         USBF_EPN_OSTL_EN		BIT(4)
+#define         USBF_EPN_DW_MASK		(0x3 << 5)
+#define         USBF_EPN_DW(_s)			((_s) << 5)
+#define         USBF_EPN_DEND			BIT(7)
+#define         USBF_EPN_CBCLR			BIT(8)
+#define         USBF_EPN_BCLR			BIT(9)
+#define         USBF_EPN_OPIDCLR		BIT(10)
+#define         USBF_EPN_IPIDCLR		BIT(11)
+#define         USBF_EPN_AUTO			BIT(16)
+#define         USBF_EPN_OVERSEL		BIT(17)
+#define         USBF_EPN_MODE_MASK		(0x3 << 24)
+#define         USBF_EPN_MODE_BULK		(0x0 << 24)
+#define         USBF_EPN_MODE_INTR		(0x1 << 24)
+#define         USBF_EPN_MODE_ISO		(0x2 << 24)
+#define         USBF_EPN_DIR0			BIT(26)
+#define         USBF_EPN_BUF_TYPE_DOUBLE	BIT(30)
+#define         USBF_EPN_EN			BIT(31)
+
+#define     USBF_REG_EPN_STATUS		0x004
+#define         USBF_EPN_IN_EMPTY		BIT(0)
+#define         USBF_EPN_IN_FULL		BIT(1)
+#define         USBF_EPN_IN_DATA		BIT(2)
+#define         USBF_EPN_IN_INT			BIT(3)
+#define         USBF_EPN_IN_STALL_INT		BIT(4)
+#define         USBF_EPN_IN_NAK_ERR_INT		BIT(5)
+#define         USBF_EPN_IN_END_INT		BIT(7)
+#define         USBF_EPN_IPID			BIT(10)
+#define         USBF_EPN_OUT_EMPTY		BIT(16)
+#define         USBF_EPN_OUT_FULL		BIT(17)
+#define         USBF_EPN_OUT_NULL_INT		BIT(18)
+#define         USBF_EPN_OUT_INT		BIT(19)
+#define         USBF_EPN_OUT_STALL_INT		BIT(20)
+#define         USBF_EPN_OUT_NAK_ERR_INT	BIT(21)
+#define         USBF_EPN_OUT_OR_INT		BIT(22)
+#define         USBF_EPN_OUT_END_INT		BIT(23)
+#define         USBF_EPN_ISO_CRC		BIT(24)
+#define         USBF_EPN_ISO_OR			BIT(26)
+#define         USBF_EPN_OUT_NOTKN		BIT(27)
+#define         USBF_EPN_ISO_OPID		BIT(28)
+#define         USBF_EPN_ISO_PIDERR		BIT(29)
+
+#define     USBF_REG_EPN_INT_ENA	0x008
+#define         USBF_EPN_IN_EN			BIT(3)
+#define         USBF_EPN_IN_STALL_EN		BIT(4)
+#define         USBF_EPN_IN_NAK_ERR_EN		BIT(5)
+#define         USBF_EPN_IN_END_EN		BIT(7)
+#define         USBF_EPN_OUT_NULL_EN		BIT(18)
+#define         USBF_EPN_OUT_EN			BIT(19)
+#define         USBF_EPN_OUT_STALL_EN		BIT(20)
+#define         USBF_EPN_OUT_NAK_ERR_EN		BIT(21)
+#define         USBF_EPN_OUT_OR_EN		BIT(22)
+#define         USBF_EPN_OUT_END_EN		BIT(23)
+
+#define     USBF_REG_EPN_DMA_CTRL	0x00C
+#define         USBF_EPN_DMAMODE0		BIT(0)
+#define         USBF_EPN_DMA_EN			BIT(4)
+#define         USBF_EPN_STOP_SET		BIT(8)
+#define         USBF_EPN_BURST_SET		BIT(9)
+#define         USBF_EPN_DEND_SET		BIT(10)
+#define         USBF_EPN_STOP_MODE		BIT(11)
+
+#define     USBF_REG_EPN_PCKT_ADRS	0x010
+#define         USBF_EPN_MPKT(_l)		((_l) << 0)
+#define         USBF_EPN_BASEAD(_a)		((_a) << 16)
+
+#define     USBF_REG_EPN_LEN_DCNT	0x014
+#define         USBF_EPN_GET_LDATA(_r)		((_r) & 0x7FF)
+#define         USBF_EPN_SET_DMACNT(_c)		((_c) << 16)
+#define         USBF_EPN_GET_DMACNT(_r)		(((_r) >> 16) & 0x1ff)
+
+#define     USBF_REG_EPN_READ		0x018
+#define     USBF_REG_EPN_WRITE		0x01C
+
+/* AHB-EPC Bridge registers */
+#define USBF_REG_AHBSCTR	0x1000
+#define USBF_REG_AHBMCTR	0x1004
+#define     USBF_SYS_WBURST_TYPE	BIT(2)
+#define     USBF_SYS_ARBITER_CTR	BIT(31)
+
+#define USBF_REG_AHBBINT	0x1008
+#define     USBF_SYS_ERR_MASTER		 (0x0F << 0)
+#define     USBF_SYS_SBUS_ERRINT0	 BIT(4)
+#define     USBF_SYS_SBUS_ERRINT1	 BIT(5)
+#define     USBF_SYS_MBUS_ERRINT	 BIT(6)
+#define     USBF_SYS_VBUS_INT		 BIT(13)
+#define     USBF_SYS_DMA_ENDINT_EPN(_n)	 (BIT(16) << (_n)) /* _n=1..15 */
+
+#define USBF_REG_AHBBINTEN	0x100C
+#define     USBF_SYS_SBUS_ERRINT0EN	  BIT(4)
+#define     USBF_SYS_SBUS_ERRINT1EN	  BIT(5)
+#define     USBF_SYS_MBUS_ERRINTEN	  BIT(6)
+#define     USBF_SYS_VBUS_INTEN		  BIT(13)
+#define     USBF_SYS_DMA_ENDINTEN_EPN(_n) (BIT(16) << (_n)) /* _n=1..15 */
+
+#define USBF_REG_EPCTR		0x1010
+#define     USBF_SYS_EPC_RST		BIT(0)
+#define     USBF_SYS_PLL_RST		BIT(2)
+#define     USBF_SYS_PLL_LOCK		BIT(4)
+#define     USBF_SYS_PLL_RESUME		BIT(5)
+#define     USBF_SYS_VBUS_LEVEL		BIT(8)
+#define     USBF_SYS_DIRPD		BIT(12)
+
+#define USBF_REG_USBSSVER	0x1020
+#define USBF_REG_USBSSCONF	0x1024
+#define    USBF_SYS_DMA_AVAILABLE(_n)	(BIT(0) << (_n)) /* _n=0..15 */
+#define    USBF_SYS_EP_AVAILABLE(_n)	(BIT(16) << (_n)) /* _n=0..15 */
+
+#define USBF_BASE_DMA_EPN(_n)	(0x1110 + (_n) * 0x010)
+/* EPn DMA registers offsets from Base USBF_BASE_DMA_EPN(n-1). n=1..15*/
+#define     USBF_REG_DMA_EPN_DCR1	0x00
+#define         USBF_SYS_EPN_REQEN		BIT(0)
+#define         USBF_SYS_EPN_DIR0		BIT(1)
+#define         USBF_SYS_EPN_SET_DMACNT(_c)	((_c) << 16)
+#define         USBF_SYS_EPN_GET_DMACNT(_r)	(((_r) >> 16) & 0x0FF)
+
+#define     USBF_REG_DMA_EPN_DCR2	0x04
+#define         USBF_SYS_EPN_MPKT(_s)		((_s) << 0)
+#define         USBF_SYS_EPN_LMPKT(_l)		((_l) << 16)
+
+#define     USBF_REG_DMA_EPN_TADR	0x08
+
+/* USB request */
+struct usbf_req {
+	struct usb_request	req;
+	struct list_head	queue;
+	unsigned int		is_zero_sent : 1;
+	unsigned int		is_mapped : 1;
+	enum {
+		USBF_XFER_START,
+		USBF_XFER_WAIT_DMA,
+		USBF_XFER_SEND_NULL,
+		USBF_XFER_WAIT_END,
+		USBF_XFER_WAIT_DMA_SHORT,
+		USBF_XFER_WAIT_BRIDGE,
+	}			xfer_step;
+	size_t			dma_size;
+};
+
+/* USB Endpoint */
+struct usbf_ep {
+	struct usb_ep		ep;
+	char			name[32];
+	struct list_head	queue;
+	unsigned int		is_processing : 1;
+	unsigned int		is_in : 1;
+	struct			usbf_udc *udc;
+	void __iomem		*regs;
+	void __iomem		*dma_regs;
+	unsigned int		id : 8;
+	unsigned int		disabled : 1;
+	unsigned int		is_wedged : 1;
+	unsigned int		delayed_status : 1;
+	u32			status;
+	void			(*bridge_on_dma_end)(struct usbf_ep *ep);
+};
+
+enum usbf_ep0state {
+	EP0_IDLE,
+	EP0_IN_DATA_PHASE,
+	EP0_OUT_DATA_PHASE,
+	EP0_OUT_STATUS_START_PHASE,
+	EP0_OUT_STATUS_PHASE,
+	EP0_OUT_STATUS_END_PHASE,
+	EP0_IN_STATUS_START_PHASE,
+	EP0_IN_STATUS_PHASE,
+	EP0_IN_STATUS_END_PHASE,
+};
+
+struct usbf_udc {
+	struct usb_gadget		gadget;
+	struct usb_gadget_driver	*driver;
+	struct device			*dev;
+	void __iomem			*regs;
+	spinlock_t			lock;
+	bool				is_remote_wakeup;
+	bool				is_usb_suspended;
+	struct usbf_ep			ep[USBF_NUM_ENDPOINTS];
+	/* for EP0 control messages */
+	enum usbf_ep0state		ep0state;
+	struct usbf_req			setup_reply;
+	u8				ep0_buf[USBF_EP0_MAX_PCKT_SIZE];
+};
+
+struct usbf_ep_info {
+	const char		*name;
+	struct usb_ep_caps	caps;
+	u16			base_addr;
+	unsigned int		is_double : 1;
+	u16			maxpacket_limit;
+};
+
+#define USBF_SINGLE_BUFFER 0
+#define USBF_DOUBLE_BUFFER 1
+#define USBF_EP_INFO(_name, _caps, _base_addr, _is_double, _maxpacket_limit)  \
+	{                                                                     \
+		.name            = _name,                                     \
+		.caps            = _caps,                                     \
+		.base_addr       = _base_addr,                                \
+		.is_double       = _is_double,                                \
+		.maxpacket_limit = _maxpacket_limit,                          \
+	}
+
+/* This table is computed from the recommended values provided in the SOC
+ * datasheet. The buffer type (single/double) and the endpoint type cannot
+ * be changed. The mapping in internal RAM (base_addr and number of words)
+ * for each endpoints depends on the max packet size and the buffer type.
+ */
+static const struct usbf_ep_info usbf_ep_info[USBF_NUM_ENDPOINTS] = {
+	/* ep0: buf @0x0000 64 bytes, fixed 32 words */
+	[0] = USBF_EP_INFO("ep0-ctrl",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_CONTROL,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x0000, USBF_SINGLE_BUFFER, USBF_EP0_MAX_PCKT_SIZE),
+	/* ep1: buf @0x0020, 2 buffers 512 bytes -> (512 * 2 / 4) words */
+	[1] = USBF_EP_INFO("ep1-bulk",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x0020, USBF_DOUBLE_BUFFER, 512),
+	/* ep2: buf @0x0120, 2 buffers 512 bytes -> (512 * 2 / 4) words */
+	[2] = USBF_EP_INFO("ep2-bulk",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x0120, USBF_DOUBLE_BUFFER, 512),
+	/* ep3: buf @0x0220, 1 buffer 512 bytes -> (512 * 2 / 4) words */
+	[3] = USBF_EP_INFO("ep3-bulk",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x0220, USBF_SINGLE_BUFFER, 512),
+	/* ep4: buf @0x02A0, 1 buffer 512 bytes -> (512 * 1 / 4) words */
+	[4] = USBF_EP_INFO("ep4-bulk",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x02A0, USBF_SINGLE_BUFFER, 512),
+	/* ep5: buf @0x0320, 1 buffer 512 bytes -> (512 * 2 / 4) words */
+	[5] = USBF_EP_INFO("ep5-bulk",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_BULK,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x0320, USBF_SINGLE_BUFFER, 512),
+	/* ep6: buf @0x03A0, 1 buffer 1024 bytes -> (1024 * 1 / 4) words */
+	[6] = USBF_EP_INFO("ep6-int",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_INT,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x03A0, USBF_SINGLE_BUFFER, 1024),
+	/* ep7: buf @0x04A0, 1 buffer 1024 bytes -> (1024 * 1 / 4) words */
+	[7] = USBF_EP_INFO("ep7-int",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_INT,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x04A0, USBF_SINGLE_BUFFER, 1024),
+	/* ep8: buf @0x0520, 1 buffer 1024 bytes -> (1024 * 1 / 4) words */
+	[8] = USBF_EP_INFO("ep8-int",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_INT,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x0520, USBF_SINGLE_BUFFER, 1024),
+	/* ep9: buf @0x0620, 1 buffer 1024 bytes -> (1024 * 1 / 4) words */
+	[9] = USBF_EP_INFO("ep9-int",
+			   USB_EP_CAPS(USB_EP_CAPS_TYPE_INT,
+				       USB_EP_CAPS_DIR_ALL),
+			   0x0620, USBF_SINGLE_BUFFER, 1024),
+	/* ep10: buf @0x0720, 2 buffers 1024 bytes -> (1024 * 2 / 4) words */
+	[10] = USBF_EP_INFO("ep10-iso",
+			    USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO,
+					USB_EP_CAPS_DIR_ALL),
+			    0x0720, USBF_DOUBLE_BUFFER, 1024),
+	/* ep11: buf @0x0920, 2 buffers 1024 bytes -> (1024 * 2 / 4) words */
+	[11] = USBF_EP_INFO("ep11-iso",
+			    USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO,
+					USB_EP_CAPS_DIR_ALL),
+			    0x0920, USBF_DOUBLE_BUFFER, 1024),
+	/* ep12: buf @0x0B20, 2 buffers 1024 bytes -> (1024 * 2 / 4) words */
+	[12] = USBF_EP_INFO("ep12-iso",
+			    USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO,
+					USB_EP_CAPS_DIR_ALL),
+			    0x0B20, USBF_DOUBLE_BUFFER, 1024),
+	/* ep13: buf @0x0D20, 2 buffers 1024 bytes -> (1024 * 2 / 4) words */
+	[13] = USBF_EP_INFO("ep13-iso",
+			    USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO,
+					USB_EP_CAPS_DIR_ALL),
+			    0x0D20, USBF_DOUBLE_BUFFER, 1024),
+	/* ep14: buf @0x0F20, 2 buffers 1024 bytes -> (1024 * 2 / 4) words */
+	[14] = USBF_EP_INFO("ep14-iso",
+			    USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO,
+					USB_EP_CAPS_DIR_ALL),
+			    0x0F20, USBF_DOUBLE_BUFFER, 1024),
+	/* ep15: buf @0x1120, 2 buffers 1024 bytes -> (1024 * 2 / 4) words */
+	[15] = USBF_EP_INFO("ep15-iso",
+			    USB_EP_CAPS(USB_EP_CAPS_TYPE_ISO,
+					USB_EP_CAPS_DIR_ALL),
+			    0x1120, USBF_DOUBLE_BUFFER, 1024),
+};
+
+static inline u32 usbf_reg_readl(struct usbf_udc *udc, uint offset)
+{
+	return readl(udc->regs + offset);
+}
+
+static inline void usbf_reg_writel(struct usbf_udc *udc, uint offset, u32 val)
+{
+	writel(val, udc->regs + offset);
+}
+
+static inline void usbf_reg_bitset(struct usbf_udc *udc, uint offset, u32 set)
+{
+	u32 tmp;
+
+	tmp = usbf_reg_readl(udc, offset);
+	tmp |= set;
+	usbf_reg_writel(udc, offset, tmp);
+}
+
+static inline void usbf_reg_bitclr(struct usbf_udc *udc, uint offset, u32 clr)
+{
+	u32 tmp;
+
+	tmp = usbf_reg_readl(udc, offset);
+	tmp &= ~clr;
+	usbf_reg_writel(udc, offset, tmp);
+}
+
+static inline void usbf_reg_clrset(struct usbf_udc *udc, uint offset,
+				   u32 clr, u32 set)
+{
+	u32 tmp;
+
+	tmp = usbf_reg_readl(udc, offset);
+	tmp &= ~clr;
+	tmp |= set;
+	usbf_reg_writel(udc, offset, tmp);
+}
+
+static inline u32 usbf_ep_reg_readl(struct usbf_ep *ep, uint offset)
+{
+	return readl(ep->regs + offset);
+}
+
+static inline void usbf_ep_reg_read_rep(struct usbf_ep *ep, uint offset,
+				       void *dst, uint count)
+{
+	readsl(ep->regs + offset, dst, count);
+}
+
+static inline void usbf_ep_reg_writel(struct usbf_ep *ep, uint offset, u32 val)
+{
+	writel(val, ep->regs + offset);
+}
+
+static inline void usbf_ep_reg_write_rep(struct usbf_ep *ep, uint offset,
+					 const void *src, uint count)
+{
+	writesl(ep->regs + offset, src, count);
+}
+
+static inline void usbf_ep_reg_bitset(struct usbf_ep *ep, uint offset, u32 set)
+{
+	u32 tmp;
+
+	tmp = usbf_ep_reg_readl(ep, offset);
+	tmp |= set;
+	usbf_ep_reg_writel(ep, offset, tmp);
+}
+
+static inline void usbf_ep_reg_bitclr(struct usbf_ep *ep, uint offset, u32 clr)
+{
+	u32 tmp;
+
+	tmp = usbf_ep_reg_readl(ep, offset);
+	tmp &= ~clr;
+	usbf_ep_reg_writel(ep, offset, tmp);
+}
+
+static inline void usbf_ep_reg_clrset(struct usbf_ep *ep, uint offset,
+				      u32 clr, u32 set)
+{
+	u32 tmp;
+
+	tmp = usbf_ep_reg_readl(ep, offset);
+	tmp &= ~clr;
+	tmp |= set;
+	usbf_ep_reg_writel(ep, offset, tmp);
+}
+
+static inline u32 usbf_ep_dma_reg_readl(struct usbf_ep *ep, uint offset)
+{
+	return readl(ep->dma_regs + offset);
+}
+
+static inline void usbf_ep_dma_reg_writel(struct usbf_ep *ep, uint offset,
+					  u32 val)
+{
+	writel(val, ep->dma_regs + offset);
+}
+
+static inline void usbf_ep_dma_reg_bitset(struct usbf_ep *ep, uint offset,
+					  u32 set)
+{
+	u32 tmp;
+
+	tmp = usbf_ep_dma_reg_readl(ep, offset);
+	tmp |= set;
+	usbf_ep_dma_reg_writel(ep, offset, tmp);
+}
+
+static inline void usbf_ep_dma_reg_bitclr(struct usbf_ep *ep, uint offset,
+					  u32 clr)
+{
+	u32 tmp;
+
+	tmp = usbf_ep_dma_reg_readl(ep, offset);
+	tmp &= ~clr;
+	usbf_ep_dma_reg_writel(ep, offset, tmp);
+}
+
+static inline void usbf_ep_dma_reg_clrset(struct usbf_ep *ep, uint offset,
+					  u32 clr, u32 set)
+{
+	u32 tmp;
+
+	tmp = usbf_ep_dma_reg_readl(ep, offset);
+	tmp &= ~clr;
+	tmp |= set;
+	usbf_ep_dma_reg_writel(ep, offset, tmp);
+}
+
+static void usbf_ep0_send_null(struct usbf_ep *ep0, bool is_data1)
+{
+	u32 set;
+
+	set = USBF_EP0_DEND;
+	if (is_data1)
+		set |= USBF_EP0_PIDCLR;
+
+	usbf_ep_reg_bitset(ep0, USBF_REG_EP0_CONTROL, set);
+}
+
+static int usbf_ep0_pio_in(struct usbf_ep *ep0, struct usbf_req *req)
+{
+	unsigned int left;
+	unsigned int nb;
+	const void *buf;
+	u32 ctrl;
+	u32 last;
+
+	left = req->req.length - req->req.actual;
+
+	if (left == 0) {
+		if (!req->is_zero_sent) {
+			if (req->req.length == 0) {
+				dev_dbg(ep0->udc->dev, "ep0 send null\n");
+				usbf_ep0_send_null(ep0, false);
+				req->is_zero_sent = 1;
+				return -EINPROGRESS;
+			}
+			if ((req->req.actual % ep0->ep.maxpacket) == 0) {
+				if (req->req.zero) {
+					dev_dbg(ep0->udc->dev, "ep0 send null\n");
+					usbf_ep0_send_null(ep0, false);
+					req->is_zero_sent = 1;
+					return -EINPROGRESS;
+				}
+			}
+		}
+		return 0;
+	}
+
+	if (left > ep0->ep.maxpacket)
+		left = ep0->ep.maxpacket;
+
+	buf = req->req.buf;
+	buf += req->req.actual;
+
+	nb = left / sizeof(u32);
+	if (nb) {
+		usbf_ep_reg_write_rep(ep0, USBF_REG_EP0_WRITE, buf, nb);
+		buf += (nb * sizeof(u32));
+		req->req.actual += (nb * sizeof(u32));
+		left -= (nb * sizeof(u32));
+	}
+	ctrl = usbf_ep_reg_readl(ep0, USBF_REG_EP0_CONTROL);
+	ctrl &= ~USBF_EP0_DW_MASK;
+	if (left) {
+		memcpy(&last, buf, left);
+		usbf_ep_reg_writel(ep0, USBF_REG_EP0_WRITE, last);
+		ctrl |= USBF_EP0_DW(left);
+		req->req.actual += left;
+	}
+	usbf_ep_reg_writel(ep0, USBF_REG_EP0_CONTROL, ctrl | USBF_EP0_DEND);
+
+	dev_dbg(ep0->udc->dev, "ep0 send %u/%u\n",
+		req->req.actual, req->req.length);
+
+	return -EINPROGRESS;
+}
+
+static int usbf_ep0_pio_out(struct usbf_ep *ep0, struct usbf_req *req)
+{
+	int req_status = 0;
+	unsigned int count;
+	unsigned int recv;
+	unsigned int left;
+	unsigned int nb;
+	void *buf;
+	u32 last;
+
+	if (ep0->status & USBF_EP0_OUT_INT) {
+		recv = usbf_ep_reg_readl(ep0, USBF_REG_EP0_LENGTH) & USBF_EP0_LDATA;
+		count = recv;
+
+		buf = req->req.buf;
+		buf += req->req.actual;
+
+		left = req->req.length - req->req.actual;
+
+		dev_dbg(ep0->udc->dev, "ep0 recv %u, left %u\n", count, left);
+
+		if (left > ep0->ep.maxpacket)
+			left = ep0->ep.maxpacket;
+
+		if (count > left) {
+			req_status = -EOVERFLOW;
+			count = left;
+		}
+
+		if (count) {
+			nb = count / sizeof(u32);
+			if (nb) {
+				usbf_ep_reg_read_rep(ep0, USBF_REG_EP0_READ,
+					buf, nb);
+				buf += (nb * sizeof(u32));
+				req->req.actual += (nb * sizeof(u32));
+				count -= (nb * sizeof(u32));
+			}
+			if (count) {
+				last = usbf_ep_reg_readl(ep0, USBF_REG_EP0_READ);
+				memcpy(buf, &last, count);
+				req->req.actual += count;
+			}
+		}
+		dev_dbg(ep0->udc->dev, "ep0 recv %u/%u\n",
+			req->req.actual, req->req.length);
+
+		if (req_status) {
+			dev_dbg(ep0->udc->dev, "ep0 req.status=%d\n", req_status);
+			req->req.status = req_status;
+			return 0;
+		}
+
+		if (recv < ep0->ep.maxpacket) {
+			dev_dbg(ep0->udc->dev, "ep0 short packet\n");
+			/* This is a short packet -> It is the end */
+			req->req.status = 0;
+			return 0;
+		}
+
+		/* The Data stage of a control transfer from an endpoint to the
+		 * host is complete when the endpoint does one of the following:
+		 * - Has transferred exactly the expected amount of data
+		 * - Transfers a packet with a payload size less than
+		 *   wMaxPacketSize or transfers a zero-length packet
+		 */
+		if (req->req.actual == req->req.length) {
+			req->req.status = 0;
+			return 0;
+		}
+	}
+
+	if (ep0->status & USBF_EP0_OUT_NULL_INT) {
+		/* NULL packet received */
+		dev_dbg(ep0->udc->dev, "ep0 null packet\n");
+		if (req->req.actual != req->req.length) {
+			req->req.status = req->req.short_not_ok ?
+					  -EREMOTEIO : 0;
+		} else {
+			req->req.status = 0;
+		}
+		return 0;
+	}
+
+	return -EINPROGRESS;
+}
+
+static void usbf_ep0_fifo_flush(struct usbf_ep *ep0)
+{
+	u32 sts;
+	int ret;
+
+	usbf_ep_reg_bitset(ep0, USBF_REG_EP0_CONTROL, USBF_EP0_BCLR);
+
+	ret = readl_poll_timeout_atomic(ep0->regs + USBF_REG_EP0_STATUS, sts,
+		(sts & (USBF_EP0_IN_DATA | USBF_EP0_IN_EMPTY)) == USBF_EP0_IN_EMPTY,
+		0,  10000);
+	if (ret)
+		dev_err(ep0->udc->dev, "ep0 flush fifo timed out\n");
+
+}
+
+static void usbf_epn_send_null(struct usbf_ep *epn)
+{
+	usbf_ep_reg_bitset(epn, USBF_REG_EPN_CONTROL, USBF_EPN_DEND);
+}
+
+static void usbf_epn_send_residue(struct usbf_ep *epn, const void *buf,
+				  unsigned int size)
+{
+	u32 tmp;
+
+	memcpy(&tmp, buf, size);
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_WRITE, tmp);
+
+	usbf_ep_reg_clrset(epn, USBF_REG_EPN_CONTROL,
+				USBF_EPN_DW_MASK,
+				USBF_EPN_DW(size) | USBF_EPN_DEND);
+}
+
+static int usbf_epn_pio_in(struct usbf_ep *epn, struct usbf_req *req)
+{
+	unsigned int left;
+	unsigned int nb;
+	const void *buf;
+
+	left = req->req.length - req->req.actual;
+
+	if (left == 0) {
+		if (!req->is_zero_sent) {
+			if (req->req.length == 0) {
+				dev_dbg(epn->udc->dev, "ep%u send_null\n", epn->id);
+				usbf_epn_send_null(epn);
+				req->is_zero_sent = 1;
+				return -EINPROGRESS;
+			}
+			if ((req->req.actual % epn->ep.maxpacket) == 0) {
+				if (req->req.zero) {
+					dev_dbg(epn->udc->dev, "ep%u send_null\n",
+						epn->id);
+					usbf_epn_send_null(epn);
+					req->is_zero_sent = 1;
+					return -EINPROGRESS;
+				}
+			}
+		}
+		return 0;
+	}
+
+	if (left > epn->ep.maxpacket)
+		left = epn->ep.maxpacket;
+
+	buf = req->req.buf;
+	buf += req->req.actual;
+
+	nb = left / sizeof(u32);
+	if (nb) {
+		usbf_ep_reg_write_rep(epn, USBF_REG_EPN_WRITE, buf, nb);
+		buf += (nb * sizeof(u32));
+		req->req.actual += (nb * sizeof(u32));
+		left -= (nb * sizeof(u32));
+	}
+
+	if (left) {
+		usbf_epn_send_residue(epn, buf, left);
+		req->req.actual += left;
+	} else {
+		usbf_ep_reg_clrset(epn, USBF_REG_EPN_CONTROL,
+					USBF_EPN_DW_MASK,
+					USBF_EPN_DEND);
+	}
+
+	dev_dbg(epn->udc->dev, "ep%u send %u/%u\n", epn->id, req->req.actual,
+		req->req.length);
+
+	return -EINPROGRESS;
+}
+
+static void usbf_epn_enable_in_end_int(struct usbf_ep *epn)
+{
+	usbf_ep_reg_bitset(epn, USBF_REG_EPN_INT_ENA, USBF_EPN_IN_END_EN);
+}
+
+static int usbf_epn_dma_in(struct usbf_ep *epn, struct usbf_req *req)
+{
+	unsigned int left;
+	u32 npkt;
+	u32 lastpkt;
+	int ret;
+
+	if (!IS_ALIGNED((uintptr_t)req->req.buf, 4)) {
+		dev_dbg(epn->udc->dev, "ep%u buf unaligned -> fallback pio\n",
+			epn->id);
+		return usbf_epn_pio_in(epn, req);
+	}
+
+	left = req->req.length - req->req.actual;
+
+	switch (req->xfer_step) {
+	default:
+	case USBF_XFER_START:
+		if (left == 0) {
+			dev_dbg(epn->udc->dev, "ep%u send null\n", epn->id);
+			usbf_epn_send_null(epn);
+			req->xfer_step = USBF_XFER_WAIT_END;
+			break;
+		}
+		if (left < 4) {
+			dev_dbg(epn->udc->dev, "ep%u send residue %u\n", epn->id,
+				left);
+			usbf_epn_send_residue(epn,
+				req->req.buf + req->req.actual, left);
+			req->req.actual += left;
+			req->xfer_step = USBF_XFER_WAIT_END;
+			break;
+		}
+
+		ret = usb_gadget_map_request(&epn->udc->gadget, &req->req, 1);
+		if (ret < 0) {
+			dev_err(epn->udc->dev, "usb_gadget_map_request failed (%d)\n",
+				ret);
+			return ret;
+		}
+		req->is_mapped = 1;
+
+		npkt = DIV_ROUND_UP(left, epn->ep.maxpacket);
+		lastpkt = (left % epn->ep.maxpacket);
+		if (lastpkt == 0)
+			lastpkt = epn->ep.maxpacket;
+		lastpkt &= ~0x3; /* DMA is done on 32bit units */
+
+		usbf_ep_dma_reg_writel(epn, USBF_REG_DMA_EPN_DCR2,
+			USBF_SYS_EPN_MPKT(epn->ep.maxpacket) | USBF_SYS_EPN_LMPKT(lastpkt));
+		usbf_ep_dma_reg_writel(epn, USBF_REG_DMA_EPN_TADR,
+			req->req.dma);
+		usbf_ep_dma_reg_writel(epn, USBF_REG_DMA_EPN_DCR1,
+			USBF_SYS_EPN_SET_DMACNT(npkt));
+		usbf_ep_dma_reg_bitset(epn, USBF_REG_DMA_EPN_DCR1,
+			USBF_SYS_EPN_REQEN);
+
+		usbf_ep_reg_writel(epn, USBF_REG_EPN_LEN_DCNT, USBF_EPN_SET_DMACNT(npkt));
+
+		usbf_ep_reg_bitset(epn, USBF_REG_EPN_CONTROL, USBF_EPN_AUTO);
+
+		/* The end of DMA transfer at the USBF level needs to be handle
+		 * after the detection of the end of DMA transfer at the brige
+		 * level.
+		 * To force this sequence, EPN_IN_END_EN will be set by the
+		 * detection of the end of transfer at bridge level (ie. bridge
+		 * interrupt).
+		 */
+		usbf_ep_reg_bitclr(epn, USBF_REG_EPN_INT_ENA,
+			USBF_EPN_IN_EN | USBF_EPN_IN_END_EN);
+		epn->bridge_on_dma_end = usbf_epn_enable_in_end_int;
+
+		/* Clear any pending IN_END interrupt */
+		usbf_ep_reg_writel(epn, USBF_REG_EPN_STATUS, ~(u32)USBF_EPN_IN_END_INT);
+
+		usbf_ep_reg_writel(epn, USBF_REG_EPN_DMA_CTRL,
+			USBF_EPN_BURST_SET | USBF_EPN_DMAMODE0);
+		usbf_ep_reg_bitset(epn, USBF_REG_EPN_DMA_CTRL,
+			USBF_EPN_DMA_EN);
+
+		req->dma_size = (npkt - 1) * epn->ep.maxpacket + lastpkt;
+
+		dev_dbg(epn->udc->dev, "ep%u dma xfer %zu\n", epn->id,
+			req->dma_size);
+
+		req->xfer_step = USBF_XFER_WAIT_DMA;
+		break;
+
+	case USBF_XFER_WAIT_DMA:
+		if (!(epn->status & USBF_EPN_IN_END_INT)) {
+			dev_dbg(epn->udc->dev, "ep%u dma not done\n", epn->id);
+			break;
+		}
+		dev_dbg(epn->udc->dev, "ep%u dma done\n", epn->id);
+
+		usb_gadget_unmap_request(&epn->udc->gadget, &req->req, 1);
+		req->is_mapped = 0;
+
+		usbf_ep_reg_bitclr(epn, USBF_REG_EPN_CONTROL, USBF_EPN_AUTO);
+
+		usbf_ep_reg_clrset(epn, USBF_REG_EPN_INT_ENA,
+			USBF_EPN_IN_END_EN,
+			USBF_EPN_IN_EN);
+
+		req->req.actual += req->dma_size;
+
+		left = req->req.length - req->req.actual;
+		if (left) {
+			usbf_ep_reg_writel(epn, USBF_REG_EPN_STATUS, ~(u32)USBF_EPN_IN_INT);
+
+			dev_dbg(epn->udc->dev, "ep%u send residue %u\n", epn->id,
+				left);
+			usbf_epn_send_residue(epn,
+				req->req.buf + req->req.actual, left);
+			req->req.actual += left;
+			req->xfer_step = USBF_XFER_WAIT_END;
+			break;
+		}
+
+		if (req->req.actual % epn->ep.maxpacket) {
+			/* last packet was a short packet. Tell the hardware to
+			 * send it right now.
+			 */
+			dev_dbg(epn->udc->dev, "ep%u send short\n", epn->id);
+			usbf_ep_reg_writel(epn, USBF_REG_EPN_STATUS,
+				~(u32)USBF_EPN_IN_INT);
+			usbf_ep_reg_bitset(epn, USBF_REG_EPN_CONTROL,
+				USBF_EPN_DEND);
+
+			req->xfer_step = USBF_XFER_WAIT_END;
+			break;
+		}
+
+		/* Last packet size was a maxpacket size
+		 * Send null packet if needed
+		 */
+		if (req->req.zero) {
+			req->xfer_step = USBF_XFER_SEND_NULL;
+			break;
+		}
+
+		/* No more action to do. Wait for the end of the USB transfer */
+		req->xfer_step = USBF_XFER_WAIT_END;
+		break;
+
+	case USBF_XFER_SEND_NULL:
+		dev_dbg(epn->udc->dev, "ep%u send null\n", epn->id);
+		usbf_epn_send_null(epn);
+		req->xfer_step = USBF_XFER_WAIT_END;
+		break;
+
+	case USBF_XFER_WAIT_END:
+		if (!(epn->status & USBF_EPN_IN_INT)) {
+			dev_dbg(epn->udc->dev, "ep%u end not done\n", epn->id);
+			break;
+		}
+		dev_dbg(epn->udc->dev, "ep%u send done %u/%u\n", epn->id,
+			req->req.actual, req->req.length);
+		req->xfer_step = USBF_XFER_START;
+		return 0;
+	}
+
+	return -EINPROGRESS;
+}
+
+static void usbf_epn_recv_residue(struct usbf_ep *epn, void *buf,
+				  unsigned int size)
+{
+	u32 last;
+
+	last = usbf_ep_reg_readl(epn, USBF_REG_EPN_READ);
+	memcpy(buf, &last, size);
+}
+
+static int usbf_epn_pio_out(struct usbf_ep *epn, struct usbf_req *req)
+{
+	int req_status = 0;
+	unsigned int count;
+	unsigned int recv;
+	unsigned int left;
+	unsigned int nb;
+	void *buf;
+
+	if (epn->status & USBF_EPN_OUT_INT) {
+		recv = USBF_EPN_GET_LDATA(
+			usbf_ep_reg_readl(epn, USBF_REG_EPN_LEN_DCNT));
+		count = recv;
+
+		buf = req->req.buf;
+		buf += req->req.actual;
+
+		left = req->req.length - req->req.actual;
+
+		dev_dbg(epn->udc->dev, "ep%u recv %u, left %u, mpkt %u\n", epn->id,
+			recv, left, epn->ep.maxpacket);
+
+		if (left > epn->ep.maxpacket)
+			left = epn->ep.maxpacket;
+
+		if (count > left) {
+			req_status = -EOVERFLOW;
+			count = left;
+		}
+
+		if (count) {
+			nb = count / sizeof(u32);
+			if (nb) {
+				usbf_ep_reg_read_rep(epn, USBF_REG_EPN_READ,
+					buf, nb);
+				buf += (nb * sizeof(u32));
+				req->req.actual += (nb * sizeof(u32));
+				count -= (nb * sizeof(u32));
+			}
+			if (count) {
+				usbf_epn_recv_residue(epn, buf, count);
+				req->req.actual += count;
+			}
+		}
+		dev_dbg(epn->udc->dev, "ep%u recv %u/%u\n", epn->id,
+			req->req.actual, req->req.length);
+
+		if (req_status) {
+			dev_dbg(epn->udc->dev, "ep%u req.status=%d\n", epn->id,
+				req_status);
+			req->req.status = req_status;
+			return 0;
+		}
+
+		if (recv < epn->ep.maxpacket) {
+			dev_dbg(epn->udc->dev, "ep%u short packet\n", epn->id);
+			/* This is a short packet -> It is the end */
+			req->req.status = 0;
+			return 0;
+		}
+
+		/* Request full -> complete */
+		if (req->req.actual == req->req.length) {
+			req->req.status = 0;
+			return 0;
+		}
+	}
+
+	if (epn->status & USBF_EPN_OUT_NULL_INT) {
+		/* NULL packet received */
+		dev_dbg(epn->udc->dev, "ep%u null packet\n", epn->id);
+		if (req->req.actual != req->req.length) {
+			req->req.status = req->req.short_not_ok ?
+					  -EREMOTEIO : 0;
+		} else {
+			req->req.status = 0;
+		}
+		return 0;
+	}
+
+	return -EINPROGRESS;
+}
+
+static void usbf_epn_enable_out_end_int(struct usbf_ep *epn)
+{
+	usbf_ep_reg_bitset(epn, USBF_REG_EPN_INT_ENA, USBF_EPN_OUT_END_EN);
+}
+
+static void usbf_epn_process_queue(struct usbf_ep *epn);
+
+static void usbf_epn_dma_out_send_dma(struct usbf_ep *epn, dma_addr_t addr, u32 npkt, bool is_short)
+{
+	usbf_ep_dma_reg_writel(epn, USBF_REG_DMA_EPN_DCR2, USBF_SYS_EPN_MPKT(epn->ep.maxpacket));
+	usbf_ep_dma_reg_writel(epn, USBF_REG_DMA_EPN_TADR, addr);
+
+	if (is_short) {
+		usbf_ep_dma_reg_writel(epn, USBF_REG_DMA_EPN_DCR1,
+				USBF_SYS_EPN_SET_DMACNT(1) | USBF_SYS_EPN_DIR0);
+		usbf_ep_dma_reg_bitset(epn, USBF_REG_DMA_EPN_DCR1,
+				USBF_SYS_EPN_REQEN);
+
+		usbf_ep_reg_writel(epn, USBF_REG_EPN_LEN_DCNT,
+				USBF_EPN_SET_DMACNT(0));
+
+		/* The end of DMA transfer at the USBF level needs to be handled
+		 * after the detection of the end of DMA transfer at the brige
+		 * level.
+		 * To force this sequence, enabling the OUT_END interrupt will
+		 * be donee by the detection of the end of transfer at bridge
+		 * level (ie. bridge interrupt).
+		 */
+		usbf_ep_reg_bitclr(epn, USBF_REG_EPN_INT_ENA,
+			USBF_EPN_OUT_EN | USBF_EPN_OUT_NULL_EN | USBF_EPN_OUT_END_EN);
+		epn->bridge_on_dma_end = usbf_epn_enable_out_end_int;
+
+		/* Clear any pending OUT_END interrupt */
+		usbf_ep_reg_writel(epn, USBF_REG_EPN_STATUS,
+			~(u32)USBF_EPN_OUT_END_INT);
+
+		usbf_ep_reg_writel(epn, USBF_REG_EPN_DMA_CTRL,
+			USBF_EPN_STOP_MODE | USBF_EPN_STOP_SET | USBF_EPN_DMAMODE0);
+		usbf_ep_reg_bitset(epn, USBF_REG_EPN_DMA_CTRL,
+			USBF_EPN_DMA_EN);
+		return;
+	}
+
+	usbf_ep_dma_reg_writel(epn, USBF_REG_DMA_EPN_DCR1,
+		USBF_SYS_EPN_SET_DMACNT(npkt) | USBF_SYS_EPN_DIR0);
+	usbf_ep_dma_reg_bitset(epn, USBF_REG_DMA_EPN_DCR1,
+		USBF_SYS_EPN_REQEN);
+
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_LEN_DCNT,
+		USBF_EPN_SET_DMACNT(npkt));
+
+	/* Here, the bridge may or may not generate an interrupt to signal the
+	 * end of DMA transfer.
+	 * Keep only OUT_END interrupt and let handle the bridge later during
+	 * the OUT_END processing.
+	 */
+	usbf_ep_reg_clrset(epn, USBF_REG_EPN_INT_ENA,
+		USBF_EPN_OUT_EN | USBF_EPN_OUT_NULL_EN,
+		USBF_EPN_OUT_END_EN);
+
+	/* Disable bridge interrupt. It will be renabled later */
+	usbf_reg_bitclr(epn->udc, USBF_REG_AHBBINTEN,
+		USBF_SYS_DMA_ENDINTEN_EPN(epn->id));
+
+	/* Clear any pending DMA_END interrupt at bridge level */
+	usbf_reg_writel(epn->udc, USBF_REG_AHBBINT,
+		USBF_SYS_DMA_ENDINT_EPN(epn->id));
+
+	/* Clear any pending OUT_END interrupt */
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_STATUS,
+		~(u32)USBF_EPN_OUT_END_INT);
+
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_DMA_CTRL,
+		USBF_EPN_STOP_MODE | USBF_EPN_STOP_SET | USBF_EPN_DMAMODE0 | USBF_EPN_BURST_SET);
+	usbf_ep_reg_bitset(epn, USBF_REG_EPN_DMA_CTRL,
+		USBF_EPN_DMA_EN);
+}
+
+static size_t usbf_epn_dma_out_complete_dma(struct usbf_ep *epn, bool is_short)
+{
+	u32 dmacnt;
+	u32 tmp;
+	int ret;
+
+	/* Restore interrupt mask */
+	usbf_ep_reg_clrset(epn, USBF_REG_EPN_INT_ENA,
+		USBF_EPN_OUT_END_EN,
+		USBF_EPN_OUT_EN | USBF_EPN_OUT_NULL_EN);
+
+	if (is_short) {
+		/* Nothing more to do when the DMA was for a short packet */
+		return 0;
+	}
+
+	/* Enable the bridge interrupt */
+	usbf_reg_bitset(epn->udc, USBF_REG_AHBBINTEN,
+		USBF_SYS_DMA_ENDINTEN_EPN(epn->id));
+
+	tmp = usbf_ep_reg_readl(epn, USBF_REG_EPN_LEN_DCNT);
+	dmacnt = USBF_EPN_GET_DMACNT(tmp);
+
+	if (dmacnt) {
+		/* Some packet were not received (halted by a short or a null
+		 * packet.
+		 * The bridge never raises an interrupt in this case.
+		 * Wait for the end of transfer at bridge level
+		 */
+		ret = readl_poll_timeout_atomic(
+			epn->dma_regs + USBF_REG_DMA_EPN_DCR1,
+			tmp, (USBF_SYS_EPN_GET_DMACNT(tmp) == dmacnt),
+			0,  10000);
+		if (ret) {
+			dev_err(epn->udc->dev, "ep%u wait bridge timed out\n",
+				epn->id);
+		}
+
+		usbf_ep_dma_reg_bitclr(epn, USBF_REG_DMA_EPN_DCR1,
+			USBF_SYS_EPN_REQEN);
+
+		/* The dmacnt value tells how many packet were not transferred
+		 * from the maximum number of packet we set for the DMA transfer.
+		 * Compute the left DMA size based on this value.
+		 */
+		return dmacnt * epn->ep.maxpacket;
+	}
+
+	return 0;
+}
+
+static int usbf_epn_dma_out(struct usbf_ep *epn, struct usbf_req *req)
+{
+	unsigned int dma_left;
+	unsigned int count;
+	unsigned int recv;
+	unsigned int left;
+	u32 npkt;
+	int ret;
+
+	if (!IS_ALIGNED((uintptr_t)req->req.buf, 4)) {
+		dev_dbg(epn->udc->dev, "ep%u buf unaligned -> fallback pio\n",
+			epn->id);
+		return usbf_epn_pio_out(epn, req);
+	}
+
+	switch (req->xfer_step) {
+	default:
+	case USBF_XFER_START:
+		if (epn->status & USBF_EPN_OUT_NULL_INT) {
+			dev_dbg(epn->udc->dev, "ep%u null packet\n", epn->id);
+			if (req->req.actual != req->req.length) {
+				req->req.status = req->req.short_not_ok ?
+					-EREMOTEIO : 0;
+			} else {
+				req->req.status = 0;
+			}
+			return 0;
+		}
+
+		if (!(epn->status & USBF_EPN_OUT_INT)) {
+			dev_dbg(epn->udc->dev, "ep%u OUT_INT not set -> spurious\n",
+				epn->id);
+			break;
+		}
+
+		recv = USBF_EPN_GET_LDATA(
+			usbf_ep_reg_readl(epn, USBF_REG_EPN_LEN_DCNT));
+		if (!recv) {
+			dev_dbg(epn->udc->dev, "ep%u recv = 0 -> spurious\n",
+				epn->id);
+			break;
+		}
+
+		left = req->req.length - req->req.actual;
+
+		dev_dbg(epn->udc->dev, "ep%u recv %u, left %u, mpkt %u\n", epn->id,
+			recv, left, epn->ep.maxpacket);
+
+		if (recv > left) {
+			dev_err(epn->udc->dev, "ep%u overflow (%u/%u)\n",
+				epn->id, recv, left);
+			req->req.status = -EOVERFLOW;
+			return -EOVERFLOW;
+		}
+
+		if (recv < epn->ep.maxpacket) {
+			/* Short packet received */
+			dev_dbg(epn->udc->dev, "ep%u short packet\n", epn->id);
+			if (recv <= 3) {
+				usbf_epn_recv_residue(epn,
+					req->req.buf + req->req.actual, recv);
+				req->req.actual += recv;
+
+				dev_dbg(epn->udc->dev, "ep%u recv done %u/%u\n",
+					epn->id, req->req.actual, req->req.length);
+
+				req->xfer_step = USBF_XFER_START;
+				return 0;
+			}
+
+			ret = usb_gadget_map_request(&epn->udc->gadget, &req->req, 0);
+			if (ret < 0) {
+				dev_err(epn->udc->dev, "map request failed (%d)\n",
+					ret);
+				return ret;
+			}
+			req->is_mapped = 1;
+
+			usbf_epn_dma_out_send_dma(epn,
+				req->req.dma + req->req.actual,
+				1, true);
+			req->dma_size = recv & ~0x3;
+
+			dev_dbg(epn->udc->dev, "ep%u dma short xfer %zu\n", epn->id,
+				req->dma_size);
+
+			req->xfer_step = USBF_XFER_WAIT_DMA_SHORT;
+			break;
+		}
+
+		ret = usb_gadget_map_request(&epn->udc->gadget, &req->req, 0);
+		if (ret < 0) {
+			dev_err(epn->udc->dev, "map request failed (%d)\n",
+				ret);
+			return ret;
+		}
+		req->is_mapped = 1;
+
+		/* Use the maximum DMA size according to the request buffer.
+		 * We will adjust the received size later at the end of the DMA
+		 * transfer with the left size computed from
+		 * usbf_epn_dma_out_complete_dma().
+		 */
+		npkt = left / epn->ep.maxpacket;
+		usbf_epn_dma_out_send_dma(epn,
+				req->req.dma + req->req.actual,
+				npkt, false);
+		req->dma_size = npkt * epn->ep.maxpacket;
+
+		dev_dbg(epn->udc->dev, "ep%u dma xfer %zu (%u)\n", epn->id,
+			req->dma_size, npkt);
+
+		req->xfer_step = USBF_XFER_WAIT_DMA;
+		break;
+
+	case USBF_XFER_WAIT_DMA_SHORT:
+		if (!(epn->status & USBF_EPN_OUT_END_INT)) {
+			dev_dbg(epn->udc->dev, "ep%u dma short not done\n", epn->id);
+			break;
+		}
+		dev_dbg(epn->udc->dev, "ep%u dma short done\n", epn->id);
+
+		usbf_epn_dma_out_complete_dma(epn, true);
+
+		usb_gadget_unmap_request(&epn->udc->gadget, &req->req, 0);
+		req->is_mapped = 0;
+
+		req->req.actual += req->dma_size;
+
+		recv = USBF_EPN_GET_LDATA(
+			usbf_ep_reg_readl(epn, USBF_REG_EPN_LEN_DCNT));
+
+		count = recv & 0x3;
+		if (count) {
+			dev_dbg(epn->udc->dev, "ep%u recv residue %u\n", epn->id,
+				count);
+			usbf_epn_recv_residue(epn,
+				req->req.buf + req->req.actual, count);
+			req->req.actual += count;
+		}
+
+		dev_dbg(epn->udc->dev, "ep%u recv done %u/%u\n", epn->id,
+			req->req.actual, req->req.length);
+
+		req->xfer_step = USBF_XFER_START;
+		return 0;
+
+	case USBF_XFER_WAIT_DMA:
+		if (!(epn->status & USBF_EPN_OUT_END_INT)) {
+			dev_dbg(epn->udc->dev, "ep%u dma not done\n", epn->id);
+			break;
+		}
+		dev_dbg(epn->udc->dev, "ep%u dma done\n", epn->id);
+
+		dma_left = usbf_epn_dma_out_complete_dma(epn, false);
+		if (dma_left) {
+			/* Adjust the final DMA size with */
+			count = req->dma_size - dma_left;
+
+			dev_dbg(epn->udc->dev, "ep%u dma xfer done %u\n", epn->id,
+				count);
+
+			req->req.actual += count;
+
+			if (epn->status & USBF_EPN_OUT_NULL_INT) {
+				/* DMA was stopped by a null packet reception */
+				dev_dbg(epn->udc->dev, "ep%u dma stopped by null pckt\n",
+					epn->id);
+				usb_gadget_unmap_request(&epn->udc->gadget,
+							 &req->req, 0);
+				req->is_mapped = 0;
+
+				usbf_ep_reg_writel(epn, USBF_REG_EPN_STATUS,
+					~(u32)USBF_EPN_OUT_NULL_INT);
+
+				if (req->req.actual != req->req.length) {
+					req->req.status = req->req.short_not_ok ?
+						  -EREMOTEIO : 0;
+				} else {
+					req->req.status = 0;
+				}
+				dev_dbg(epn->udc->dev, "ep%u recv done %u/%u\n",
+					epn->id, req->req.actual, req->req.length);
+				req->xfer_step = USBF_XFER_START;
+				return 0;
+			}
+
+			recv = USBF_EPN_GET_LDATA(
+				usbf_ep_reg_readl(epn, USBF_REG_EPN_LEN_DCNT));
+			left = req->req.length - req->req.actual;
+			if (recv > left) {
+				dev_err(epn->udc->dev,
+					"ep%u overflow (%u/%u)\n", epn->id,
+					recv, left);
+				req->req.status = -EOVERFLOW;
+				usb_gadget_unmap_request(&epn->udc->gadget,
+							 &req->req, 0);
+				req->is_mapped = 0;
+
+				req->xfer_step = USBF_XFER_START;
+				return -EOVERFLOW;
+			}
+
+			if (recv > 3) {
+				usbf_epn_dma_out_send_dma(epn,
+					req->req.dma + req->req.actual,
+					1, true);
+				req->dma_size = recv & ~0x3;
+
+				dev_dbg(epn->udc->dev, "ep%u dma short xfer %zu\n",
+					epn->id, req->dma_size);
+
+				req->xfer_step = USBF_XFER_WAIT_DMA_SHORT;
+				break;
+			}
+
+			usb_gadget_unmap_request(&epn->udc->gadget, &req->req, 0);
+			req->is_mapped = 0;
+
+			count = recv & 0x3;
+			if (count) {
+				dev_dbg(epn->udc->dev, "ep%u recv residue %u\n",
+					epn->id, count);
+				usbf_epn_recv_residue(epn,
+					req->req.buf + req->req.actual, count);
+				req->req.actual += count;
+			}
+
+			dev_dbg(epn->udc->dev, "ep%u recv done %u/%u\n", epn->id,
+				req->req.actual, req->req.length);
+
+			req->xfer_step = USBF_XFER_START;
+			return 0;
+		}
+
+		/* Process queue at bridge interrupt only */
+		usbf_ep_reg_bitclr(epn, USBF_REG_EPN_INT_ENA,
+			USBF_EPN_OUT_END_EN | USBF_EPN_OUT_EN | USBF_EPN_OUT_NULL_EN);
+		epn->status = 0;
+		epn->bridge_on_dma_end = usbf_epn_process_queue;
+
+		req->xfer_step = USBF_XFER_WAIT_BRIDGE;
+		break;
+
+	case USBF_XFER_WAIT_BRIDGE:
+		dev_dbg(epn->udc->dev, "ep%u bridge transfers done\n", epn->id);
+
+		/* Restore interrupt mask */
+		usbf_ep_reg_clrset(epn, USBF_REG_EPN_INT_ENA,
+			USBF_EPN_OUT_END_EN,
+			USBF_EPN_OUT_EN | USBF_EPN_OUT_NULL_EN);
+
+		usb_gadget_unmap_request(&epn->udc->gadget, &req->req, 0);
+		req->is_mapped = 0;
+
+		req->req.actual += req->dma_size;
+
+		req->xfer_step = USBF_XFER_START;
+		left = req->req.length - req->req.actual;
+		if (!left) {
+			/* No more data can be added to the buffer */
+			dev_dbg(epn->udc->dev, "ep%u recv done %u/%u\n", epn->id,
+				req->req.actual, req->req.length);
+			return 0;
+		}
+		dev_dbg(epn->udc->dev, "ep%u recv done %u/%u, wait more data\n",
+			epn->id, req->req.actual, req->req.length);
+		break;
+	}
+
+	return -EINPROGRESS;
+}
+
+static void usbf_epn_dma_stop(struct usbf_ep *epn)
+{
+	usbf_ep_dma_reg_bitclr(epn, USBF_REG_DMA_EPN_DCR1, USBF_SYS_EPN_REQEN);
+
+	/* In the datasheet:
+	 *   If EP[m]_REQEN = 0b is set during DMA transfer, AHB-EPC stops DMA
+	 *   after 1 packet transfer completed.
+	 *   Therefore, wait sufficient time for ensuring DMA transfer
+	 *   completion. The WAIT time depends on the system, especially AHB
+	 *   bus activity
+	 * So arbitrary 10ms would be sufficient.
+	 */
+	mdelay(10);
+
+	usbf_ep_reg_bitclr(epn, USBF_REG_EPN_DMA_CTRL, USBF_EPN_DMA_EN);
+}
+
+static void usbf_epn_dma_abort(struct usbf_ep *epn,  struct usbf_req *req)
+{
+	dev_dbg(epn->udc->dev, "ep%u %s dma abort\n", epn->id,
+		epn->is_in ? "in" : "out");
+
+	epn->bridge_on_dma_end = NULL;
+
+	usbf_epn_dma_stop(epn);
+
+	usb_gadget_unmap_request(&epn->udc->gadget, &req->req,
+				 epn->is_in ? 1 : 0);
+	req->is_mapped = 0;
+
+	usbf_ep_reg_bitclr(epn, USBF_REG_EPN_CONTROL, USBF_EPN_AUTO);
+
+	if (epn->is_in) {
+		usbf_ep_reg_clrset(epn, USBF_REG_EPN_INT_ENA,
+			USBF_EPN_IN_END_EN,
+			USBF_EPN_IN_EN);
+	} else {
+		usbf_ep_reg_clrset(epn, USBF_REG_EPN_INT_ENA,
+			USBF_EPN_OUT_END_EN,
+			USBF_EPN_OUT_EN | USBF_EPN_OUT_NULL_EN);
+	}
+
+	/* As dma is stopped, be sure that no DMA interrupt are pending */
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_STATUS,
+		USBF_EPN_IN_END_INT | USBF_EPN_OUT_END_INT);
+
+	usbf_reg_writel(epn->udc, USBF_REG_AHBBINT, USBF_SYS_DMA_ENDINT_EPN(epn->id));
+
+	/* Enable DMA interrupt the bridge level */
+	usbf_reg_bitset(epn->udc, USBF_REG_AHBBINTEN,
+		USBF_SYS_DMA_ENDINTEN_EPN(epn->id));
+
+	/* Reset transfer step */
+	req->xfer_step = USBF_XFER_START;
+}
+
+static void usbf_epn_fifo_flush(struct usbf_ep *epn)
+{
+	u32 ctrl;
+	u32 sts;
+	int ret;
+
+	dev_dbg(epn->udc->dev, "ep%u %s fifo flush\n", epn->id,
+		epn->is_in ? "in" : "out");
+
+	ctrl = usbf_ep_reg_readl(epn, USBF_REG_EPN_CONTROL);
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_CONTROL, ctrl | USBF_EPN_BCLR);
+
+	if (ctrl & USBF_EPN_DIR0)
+		return;
+
+	ret = readl_poll_timeout_atomic(epn->regs + USBF_REG_EPN_STATUS, sts,
+		(sts & (USBF_EPN_IN_DATA | USBF_EPN_IN_EMPTY)) == USBF_EPN_IN_EMPTY,
+		0,  10000);
+	if (ret)
+		dev_err(epn->udc->dev, "ep%u flush fifo timed out\n", epn->id);
+}
+
+static void usbf_ep_req_done(struct usbf_ep *ep, struct usbf_req *req,
+			     int status)
+{
+	list_del_init(&req->queue);
+
+	if (status) {
+		req->req.status = status;
+	} else {
+		if (req->req.status == -EINPROGRESS)
+			req->req.status = status;
+	}
+
+	dev_dbg(ep->udc->dev, "ep%u %s req done length %u/%u, status=%d\n", ep->id,
+		ep->is_in ? "in" : "out",
+		req->req.actual, req->req.length, req->req.status);
+
+	if (req->is_mapped)
+		usbf_epn_dma_abort(ep, req);
+
+	spin_unlock(&ep->udc->lock);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
+	spin_lock(&ep->udc->lock);
+}
+
+static void usbf_ep_nuke(struct usbf_ep *ep, int status)
+{
+	struct usbf_req *req;
+
+	dev_dbg(ep->udc->dev, "ep%u %s nuke status %d\n", ep->id,
+		ep->is_in ? "in" : "out",
+		status);
+
+	while (!list_empty(&ep->queue)) {
+		req = list_first_entry(&ep->queue, struct usbf_req, queue);
+		usbf_ep_req_done(ep, req, status);
+	}
+
+	if (ep->id == 0)
+		usbf_ep0_fifo_flush(ep);
+	else
+		usbf_epn_fifo_flush(ep);
+}
+
+static bool usbf_ep_is_stalled(struct usbf_ep *ep)
+{
+	u32 ctrl;
+
+	if (ep->id == 0) {
+		ctrl = usbf_ep_reg_readl(ep, USBF_REG_EP0_CONTROL);
+		return (ctrl & USBF_EP0_STL) ? true : false;
+	}
+
+	ctrl = usbf_ep_reg_readl(ep, USBF_REG_EPN_CONTROL);
+	if (ep->is_in)
+		return (ctrl & USBF_EPN_ISTL) ? true : false;
+
+	return (ctrl & USBF_EPN_OSTL) ? true : false;
+}
+
+static int usbf_epn_start_queue(struct usbf_ep *epn)
+{
+	struct usbf_req *req;
+	int ret;
+
+	if (usbf_ep_is_stalled(epn))
+		return 0;
+
+	req = list_first_entry_or_null(&epn->queue, struct usbf_req, queue);
+
+	if (epn->is_in) {
+		if (req && !epn->is_processing) {
+			ret = epn->dma_regs ?
+				usbf_epn_dma_in(epn, req) :
+				usbf_epn_pio_in(epn, req);
+			if (ret != -EINPROGRESS) {
+				dev_err(epn->udc->dev,
+					"queued next request not in progress\n");
+					/* The request cannot be completed (ie
+					 * ret == 0) on the first call.
+					 * stall and nuke the endpoint
+					 */
+				return ret ? ret : -EIO;
+			}
+		}
+	} else {
+		if (req) {
+			/* Clear ONAK to accept OUT tokens */
+			usbf_ep_reg_bitclr(epn, USBF_REG_EPN_CONTROL,
+				USBF_EPN_ONAK);
+
+			/* Enable interrupts */
+			usbf_ep_reg_bitset(epn, USBF_REG_EPN_INT_ENA,
+				USBF_EPN_OUT_INT | USBF_EPN_OUT_NULL_INT);
+		} else {
+			/* Disable incoming data and interrupt.
+			 * They will be enable on next usb_eb_queue call
+			 */
+			usbf_ep_reg_bitset(epn, USBF_REG_EPN_CONTROL,
+				USBF_EPN_ONAK);
+			usbf_ep_reg_bitclr(epn, USBF_REG_EPN_INT_ENA,
+				USBF_EPN_OUT_INT | USBF_EPN_OUT_NULL_INT);
+		}
+	}
+	return 0;
+}
+
+static int usbf_ep_process_queue(struct usbf_ep *ep)
+{
+	int (*usbf_ep_xfer)(struct usbf_ep *ep, struct usbf_req *req);
+	struct usbf_req *req;
+	int is_processing;
+	int ret;
+
+	if (ep->is_in) {
+		usbf_ep_xfer = usbf_ep0_pio_in;
+		if (ep->id) {
+			usbf_ep_xfer = ep->dma_regs ?
+					usbf_epn_dma_in : usbf_epn_pio_in;
+		}
+	} else {
+		usbf_ep_xfer = usbf_ep0_pio_out;
+		if (ep->id) {
+			usbf_ep_xfer = ep->dma_regs ?
+					usbf_epn_dma_out : usbf_epn_pio_out;
+		}
+	}
+
+	req = list_first_entry_or_null(&ep->queue, struct usbf_req, queue);
+	if (!req) {
+		dev_err(ep->udc->dev,
+			"no request available for ep%u %s process\n", ep->id,
+			ep->is_in ? "in" : "out");
+		return -ENOENT;
+	}
+
+	do {
+		/* Were going to read the FIFO for this current request.
+		 * NAK any other incoming data to avoid a race condition if no
+		 * more request are available.
+		 */
+		if (!ep->is_in && ep->id != 0) {
+			usbf_ep_reg_bitset(ep, USBF_REG_EPN_CONTROL,
+				USBF_EPN_ONAK);
+		}
+
+		ret = usbf_ep_xfer(ep, req);
+		if (ret == -EINPROGRESS) {
+			if (!ep->is_in && ep->id != 0) {
+				/* The current request needs more data.
+				 * Allow incoming data
+				 */
+				usbf_ep_reg_bitclr(ep, USBF_REG_EPN_CONTROL,
+					USBF_EPN_ONAK);
+			}
+			return ret;
+		}
+
+		is_processing = ep->is_processing;
+		ep->is_processing = 1;
+		usbf_ep_req_done(ep, req, ret);
+		ep->is_processing = is_processing;
+
+		if (ret) {
+			/* An error was detected during the request transfer.
+			 * Any pending DMA transfers were aborted by the
+			 * usbf_ep_req_done() call.
+			 * It's time to flush the fifo
+			 */
+			if (ep->id == 0)
+				usbf_ep0_fifo_flush(ep);
+			else
+				usbf_epn_fifo_flush(ep);
+		}
+
+		req = list_first_entry_or_null(&ep->queue, struct usbf_req,
+					       queue);
+
+		if (ep->is_in)
+			continue;
+
+		if (ep->id != 0) {
+			if (req) {
+				/* An other request is available.
+				 * Allow incoming data
+				 */
+				usbf_ep_reg_bitclr(ep, USBF_REG_EPN_CONTROL,
+					USBF_EPN_ONAK);
+			} else {
+				/* No request queued. Disable interrupts.
+				 * They will be enabled on usb_ep_queue
+				 */
+				usbf_ep_reg_bitclr(ep, USBF_REG_EPN_INT_ENA,
+					USBF_EPN_OUT_INT | USBF_EPN_OUT_NULL_INT);
+			}
+		}
+		/* Do not recall usbf_ep_xfer() */
+		return req ? -EINPROGRESS : 0;
+
+	} while (req);
+
+	return 0;
+}
+
+static void usbf_ep_stall(struct usbf_ep *ep, bool stall)
+{
+	struct usbf_req *first;
+
+	dev_dbg(ep->udc->dev, "ep%u %s %s\n", ep->id,
+		ep->is_in ? "in" : "out",
+		stall ? "stall" : "unstall");
+
+	if (ep->id == 0) {
+		if (stall)
+			usbf_ep_reg_bitset(ep, USBF_REG_EP0_CONTROL, USBF_EP0_STL);
+		else
+			usbf_ep_reg_bitclr(ep, USBF_REG_EP0_CONTROL, USBF_EP0_STL);
+		return;
+	}
+
+	if (stall) {
+		if (ep->is_in)
+			usbf_ep_reg_bitset(ep, USBF_REG_EPN_CONTROL,
+				USBF_EPN_ISTL);
+		else
+			usbf_ep_reg_bitset(ep, USBF_REG_EPN_CONTROL,
+				USBF_EPN_OSTL | USBF_EPN_OSTL_EN);
+	} else {
+		first = list_first_entry_or_null(&ep->queue, struct usbf_req, queue);
+		if (first && first->is_mapped) {
+			/* This can appear if the host halts an endpoint using
+			 * SET_FEATURE and then un-halts the endpoint
+			 */
+			usbf_epn_dma_abort(ep, first);
+		}
+		usbf_epn_fifo_flush(ep);
+		if (ep->is_in) {
+			usbf_ep_reg_clrset(ep, USBF_REG_EPN_CONTROL,
+				USBF_EPN_ISTL,
+				USBF_EPN_IPIDCLR);
+		} else {
+			usbf_ep_reg_clrset(ep, USBF_REG_EPN_CONTROL,
+				USBF_EPN_OSTL,
+				USBF_EPN_OSTL_EN | USBF_EPN_OPIDCLR);
+		}
+		usbf_epn_start_queue(ep);
+	}
+}
+
+static void usbf_ep0_enable(struct usbf_ep *ep0)
+{
+	usbf_ep_reg_writel(ep0, USBF_REG_EP0_CONTROL, USBF_EP0_INAK_EN | USBF_EP0_BCLR);
+
+	usbf_ep_reg_writel(ep0, USBF_REG_EP0_INT_ENA,
+		USBF_EP0_SETUP_EN | USBF_EP0_STG_START_EN | USBF_EP0_STG_END_EN |
+		USBF_EP0_OUT_EN | USBF_EP0_OUT_NULL_EN | USBF_EP0_IN_EN);
+
+	ep0->udc->ep0state = EP0_IDLE;
+	ep0->disabled = 0;
+
+	/* enable interrupts for the ep0 */
+	usbf_reg_bitset(ep0->udc, USBF_REG_USB_INT_ENA, USBF_USB_EPN_EN(0));
+}
+
+static int usbf_epn_enable(struct usbf_ep *epn)
+{
+	u32 base_addr;
+	u32 ctrl;
+
+	base_addr = usbf_ep_info[epn->id].base_addr;
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_PCKT_ADRS,
+		USBF_EPN_BASEAD(base_addr) | USBF_EPN_MPKT(epn->ep.maxpacket));
+
+	/* OUT transfer interrupt are enabled during usb_ep_queue */
+	if (epn->is_in) {
+		/* Will be changed in DMA processing */
+		usbf_ep_reg_writel(epn, USBF_REG_EPN_INT_ENA, USBF_EPN_IN_EN);
+	}
+
+	/* Clear, set endpoint direction, set IN/OUT STL, and enable
+	 * Send NAK for Data out as request are not queued yet
+	 */
+	ctrl = USBF_EPN_EN | USBF_EPN_BCLR;
+	if (epn->is_in)
+		ctrl |= USBF_EPN_OSTL | USBF_EPN_OSTL_EN;
+	else
+		ctrl |= USBF_EPN_DIR0 | USBF_EPN_ISTL | USBF_EPN_OSTL_EN | USBF_EPN_ONAK;
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_CONTROL, ctrl);
+
+	return 0;
+}
+
+static int usbf_ep_enable(struct usb_ep *_ep,
+			  const struct usb_endpoint_descriptor *desc)
+{
+	struct usbf_ep *ep = container_of(_ep, struct usbf_ep, ep);
+	struct usbf_udc *udc = ep->udc;
+	unsigned long flags;
+	int ret;
+
+	if (ep->id == 0)
+		return -EINVAL;
+
+	if (!desc || desc->bDescriptorType != USB_DT_ENDPOINT)
+		return -EINVAL;
+
+	dev_dbg(ep->udc->dev, "ep%u %s mpkts %d\n", ep->id,
+		usb_endpoint_dir_in(desc) ? "in" : "out",
+		usb_endpoint_maxp(desc));
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+	ep->is_in = usb_endpoint_dir_in(desc);
+	ep->ep.maxpacket = usb_endpoint_maxp(desc);
+
+	ret = usbf_epn_enable(ep);
+	if (ret)
+		goto end;
+
+	ep->disabled = 0;
+
+	/* enable interrupts for this endpoint */
+	usbf_reg_bitset(udc, USBF_REG_USB_INT_ENA, USBF_USB_EPN_EN(ep->id));
+
+	/* enable DMA interrupt at bridge level if DMA is used */
+	if (ep->dma_regs) {
+		ep->bridge_on_dma_end = NULL;
+		usbf_reg_bitset(udc, USBF_REG_AHBBINTEN,
+			USBF_SYS_DMA_ENDINTEN_EPN(ep->id));
+	}
+
+	ret = 0;
+end:
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+	return ret;
+}
+
+static int usbf_epn_disable(struct usbf_ep *epn)
+{
+	/* Disable interrupts */
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_INT_ENA, 0);
+
+	/* Disable endpoint */
+	usbf_ep_reg_bitclr(epn, USBF_REG_EPN_CONTROL, USBF_EPN_EN);
+
+	/* remove anything that was pending */
+	usbf_ep_nuke(epn, -ESHUTDOWN);
+
+	return 0;
+}
+
+static int usbf_ep_disable(struct usb_ep *_ep)
+{
+	struct usbf_ep *ep = container_of(_ep, struct usbf_ep, ep);
+	struct usbf_udc *udc = ep->udc;
+	unsigned long flags;
+	int ret;
+
+	if (ep->id == 0)
+		return -EINVAL;
+
+	dev_dbg(ep->udc->dev, "ep%u %s mpkts %d\n", ep->id,
+		ep->is_in ? "in" : "out", ep->ep.maxpacket);
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+	ep->disabled = 1;
+	/* Disable DMA interrupt */
+	if (ep->dma_regs) {
+		usbf_reg_bitclr(udc, USBF_REG_AHBBINTEN,
+			USBF_SYS_DMA_ENDINTEN_EPN(ep->id));
+		ep->bridge_on_dma_end = NULL;
+	}
+	/* disable interrupts for this endpoint */
+	usbf_reg_bitclr(udc, USBF_REG_USB_INT_ENA, USBF_USB_EPN_EN(ep->id));
+	/* and the endpoint itself */
+	ret = usbf_epn_disable(ep);
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+
+	return ret;
+}
+
+static int usbf_ep0_queue(struct usbf_ep *ep0, struct usbf_req *req,
+			  gfp_t gfp_flags)
+{
+	int ret;
+
+	req->req.actual = 0;
+	req->req.status = -EINPROGRESS;
+	req->is_zero_sent = 0;
+
+	list_add_tail(&req->queue, &ep0->queue);
+
+	if (ep0->udc->ep0state == EP0_IN_STATUS_START_PHASE)
+		return 0;
+
+	if (!ep0->is_in)
+		return 0;
+
+	if (ep0->udc->ep0state == EP0_IN_STATUS_PHASE) {
+		if (req->req.length) {
+			dev_err(ep0->udc->dev,
+				"request lng %u for ep0 in status phase\n",
+				req->req.length);
+			return -EINVAL;
+		}
+		ep0->delayed_status = 0;
+	}
+	if (!ep0->is_processing) {
+		ret = usbf_ep0_pio_in(ep0, req);
+		if (ret != -EINPROGRESS) {
+			dev_err(ep0->udc->dev,
+				"queued request not in progress\n");
+			/* The request cannot be completed (ie
+			 * ret == 0) on the first call
+			 */
+			return ret ? ret : -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int usbf_epn_queue(struct usbf_ep *ep, struct usbf_req *req,
+			  gfp_t gfp_flags)
+{
+	int was_empty;
+	int ret;
+
+	if (ep->disabled) {
+		dev_err(ep->udc->dev, "ep%u request queue while disable\n",
+			ep->id);
+		return -ESHUTDOWN;
+	}
+
+	req->req.actual = 0;
+	req->req.status = -EINPROGRESS;
+	req->is_zero_sent = 0;
+	req->xfer_step = USBF_XFER_START;
+
+	was_empty = list_empty(&ep->queue);
+	list_add_tail(&req->queue, &ep->queue);
+	if (was_empty) {
+		ret = usbf_epn_start_queue(ep);
+		if (ret)
+			return ret;
+	}
+	return 0;
+}
+
+static int usbf_ep_queue(struct usb_ep *_ep, struct usb_request *_req,
+			 gfp_t gfp_flags)
+{
+	struct usbf_req *req = container_of(_req, struct usbf_req, req);
+	struct usbf_ep *ep = container_of(_ep, struct usbf_ep, ep);
+	struct usbf_udc *udc = ep->udc;
+	unsigned long flags;
+	int ret;
+
+	if (!_req || !_req->buf)
+		return -EINVAL;
+
+	if (!udc || !udc->driver)
+		return -EINVAL;
+
+	dev_dbg(ep->udc->dev, "ep%u %s req queue length %u, zero %u, short_not_ok %u\n",
+		ep->id, ep->is_in ? "in" : "out",
+		req->req.length, req->req.zero, req->req.short_not_ok);
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+	if (ep->id == 0)
+		ret = usbf_ep0_queue(ep, req, gfp_flags);
+	else
+		ret = usbf_epn_queue(ep, req, gfp_flags);
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+	return ret;
+}
+
+static int usbf_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct usbf_req *req = container_of(_req, struct usbf_req, req);
+	struct usbf_ep *ep = container_of(_ep, struct usbf_ep, ep);
+	unsigned long flags;
+	int is_processing;
+	int first;
+	int ret;
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+
+	dev_dbg(ep->udc->dev, "ep%u %s req dequeue length %u/%u\n",
+		ep->id, ep->is_in ? "in" : "out",
+		req->req.actual, req->req.length);
+
+	first = list_is_first(&req->queue, &ep->queue);
+
+	/* Complete the request but avoid any operation that could be done
+	 * if a new request is queued during the request completion
+	 */
+	is_processing = ep->is_processing;
+	ep->is_processing = 1;
+	usbf_ep_req_done(ep, req, -ECONNRESET);
+	ep->is_processing = is_processing;
+
+	if (first) {
+		/* The first item in the list was dequeued.
+		 * This item could already be submitted to the hardware.
+		 * So, flush the fifo
+		 */
+		if (ep->id)
+			usbf_epn_fifo_flush(ep);
+		else
+			usbf_ep0_fifo_flush(ep);
+	}
+
+	if (ep->id == 0) {
+		/* We dequeue a request on ep0. On this endpoint, we can have
+		 * 1 request related to the data stage and/or 1 request
+		 * related to the status stage.
+		 * We dequeue one of them and so the USB control transaction
+		 * is no more coherent. The simple way to be consistent after
+		 * dequeuing is to stall and nuke the endpoint and wait the
+		 * next SETUP packet.
+		 */
+		usbf_ep_stall(ep, true);
+		usbf_ep_nuke(ep, -ECONNRESET);
+		ep->udc->ep0state = EP0_IDLE;
+		goto end;
+	}
+
+	if (!first)
+		goto end;
+
+	ret = usbf_epn_start_queue(ep);
+	if (ret) {
+		usbf_ep_stall(ep, true);
+		usbf_ep_nuke(ep, -EIO);
+	}
+end:
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+	return 0;
+}
+
+static struct usb_request *usbf_ep_alloc_request(struct usb_ep *_ep,
+						 gfp_t gfp_flags)
+{
+	struct usbf_req *req;
+
+	if (!_ep)
+		return NULL;
+
+	req = kzalloc(sizeof(*req), gfp_flags);
+	if (!req)
+		return NULL;
+
+	INIT_LIST_HEAD(&req->queue);
+
+	return &req->req;
+}
+
+static void usbf_ep_free_request(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct usbf_req *req;
+	unsigned long flags;
+	struct usbf_ep *ep;
+
+	if (!_ep || !_req)
+		return;
+
+	req = container_of(_req, struct usbf_req, req);
+	ep = container_of(_ep, struct usbf_ep, ep);
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+	list_del_init(&req->queue);
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+	kfree(req);
+}
+
+static int usbf_ep_set_halt(struct usb_ep *_ep, int halt)
+{
+	struct usbf_ep *ep = container_of(_ep, struct usbf_ep, ep);
+	unsigned long flags;
+	int ret;
+
+	if (ep->id == 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+
+	if (!list_empty(&ep->queue)) {
+		ret = -EAGAIN;
+		goto end;
+	}
+
+	usbf_ep_stall(ep, halt);
+	if (!halt)
+		ep->is_wedged = 0;
+
+	ret = 0;
+end:
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+
+	return ret;
+}
+
+static int usbf_ep_set_wedge(struct usb_ep *_ep)
+{
+	struct usbf_ep *ep = container_of(_ep, struct usbf_ep, ep);
+	unsigned long flags;
+	int ret;
+
+	if (ep->id == 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ep->udc->lock, flags);
+	if (!list_empty(&ep->queue)) {
+		ret = -EAGAIN;
+		goto end;
+	}
+	usbf_ep_stall(ep, 1);
+	ep->is_wedged = 1;
+
+	ret = 0;
+end:
+	spin_unlock_irqrestore(&ep->udc->lock, flags);
+	return ret;
+}
+
+static struct usb_ep_ops usbf_ep_ops = {
+	.enable = usbf_ep_enable,
+	.disable = usbf_ep_disable,
+	.queue = usbf_ep_queue,
+	.dequeue = usbf_ep_dequeue,
+	.set_halt = usbf_ep_set_halt,
+	.set_wedge = usbf_ep_set_wedge,
+	.alloc_request = usbf_ep_alloc_request,
+	.free_request = usbf_ep_free_request,
+};
+
+static void usbf_ep0_req_complete(struct usb_ep *_ep, struct usb_request *_req)
+{
+}
+
+static void usbf_ep0_fill_req(struct usbf_ep *ep0, struct usbf_req *req,
+			      void *buf, unsigned int length,
+			      void (*complete)(struct usb_ep *_ep,
+					       struct usb_request *_req))
+{
+	if (buf && length)
+		memcpy(ep0->udc->ep0_buf, buf, length);
+
+	req->req.buf = ep0->udc->ep0_buf;
+	req->req.length = length;
+	req->req.dma = 0;
+	req->req.zero = true;
+	req->req.complete = complete ? complete : usbf_ep0_req_complete;
+	req->req.status = -EINPROGRESS;
+	req->req.context = NULL;
+	req->req.actual = 0;
+}
+
+static struct usbf_ep *usbf_get_ep_by_addr(struct usbf_udc *udc, u8 address)
+{
+	struct usbf_ep *ep;
+	unsigned int i;
+
+	if ((address & USB_ENDPOINT_NUMBER_MASK) == 0)
+		return &udc->ep[0];
+
+	for (i = 1; i < ARRAY_SIZE(udc->ep); i++) {
+		ep = &udc->ep[i];
+
+		if (!ep->ep.desc)
+			continue;
+
+		if (ep->ep.desc->bEndpointAddress == address)
+			return ep;
+	}
+
+	return NULL;
+}
+
+static int usbf_req_delegate(struct usbf_udc *udc,
+			     const struct usb_ctrlrequest *ctrlrequest)
+{
+	int ret;
+
+	spin_unlock(&udc->lock);
+	ret = udc->driver->setup(&udc->gadget, ctrlrequest);
+	spin_lock(&udc->lock);
+	if (ret < 0) {
+		dev_dbg(udc->dev, "udc driver setup failed %d\n", ret);
+		return ret;
+	}
+	if (ret == USB_GADGET_DELAYED_STATUS) {
+		dev_dbg(udc->dev, "delayed status set\n");
+		udc->ep[0].delayed_status = 1;
+		return 0;
+	}
+	return ret;
+}
+
+static int usbf_req_get_status(struct usbf_udc *udc,
+			       const struct usb_ctrlrequest *ctrlrequest)
+{
+	struct usbf_ep *ep;
+	u16 status_data;
+	u16 wLength;
+	u16 wValue;
+	u16 wIndex;
+
+	wValue  = le16_to_cpu(ctrlrequest->wValue);
+	wLength = le16_to_cpu(ctrlrequest->wLength);
+	wIndex  = le16_to_cpu(ctrlrequest->wIndex);
+
+	switch (ctrlrequest->bRequestType) {
+	case USB_DIR_IN | USB_RECIP_DEVICE | USB_TYPE_STANDARD:
+		if ((wValue != 0) || (wIndex != 0) || (wLength != 2))
+			goto delegate;
+
+		status_data = 0;
+		if (udc->gadget.is_selfpowered)
+			status_data |= BIT(USB_DEVICE_SELF_POWERED);
+
+		if (udc->is_remote_wakeup)
+			status_data |= BIT(USB_DEVICE_REMOTE_WAKEUP);
+
+		break;
+
+	case USB_DIR_IN | USB_RECIP_ENDPOINT | USB_TYPE_STANDARD:
+		if ((wValue != 0) || (wLength != 2))
+			goto delegate;
+
+		ep = usbf_get_ep_by_addr(udc, wIndex);
+		if (!ep)
+			return -EINVAL;
+
+		status_data = 0;
+		if (usbf_ep_is_stalled(ep))
+			status_data |= cpu_to_le16(1);
+		break;
+
+	case USB_DIR_IN | USB_RECIP_INTERFACE | USB_TYPE_STANDARD:
+		if ((wValue != 0) || (wLength != 2))
+			goto delegate;
+		status_data = 0;
+		break;
+
+	default:
+		goto delegate;
+	}
+
+	usbf_ep0_fill_req(&udc->ep[0], &udc->setup_reply, &status_data,
+			  sizeof(status_data), NULL);
+	usbf_ep0_queue(&udc->ep[0], &udc->setup_reply, GFP_ATOMIC);
+
+	return 0;
+
+delegate:
+	return usbf_req_delegate(udc, ctrlrequest);
+}
+
+static int usbf_req_clear_set_feature(struct usbf_udc *udc,
+				      const struct usb_ctrlrequest *ctrlrequest,
+				      bool is_set)
+{
+	struct usbf_ep *ep;
+	u16 wLength;
+	u16 wValue;
+	u16 wIndex;
+
+	wValue  = le16_to_cpu(ctrlrequest->wValue);
+	wLength = le16_to_cpu(ctrlrequest->wLength);
+	wIndex  = le16_to_cpu(ctrlrequest->wIndex);
+
+	switch (ctrlrequest->bRequestType) {
+	case USB_DIR_OUT | USB_RECIP_DEVICE:
+		if ((wIndex != 0) || (wLength != 0))
+			goto delegate;
+
+		if (wValue != cpu_to_le16(USB_DEVICE_REMOTE_WAKEUP))
+			goto delegate;
+
+		udc->is_remote_wakeup = is_set;
+		break;
+
+	case USB_DIR_OUT | USB_RECIP_ENDPOINT:
+		if (wLength != 0)
+			goto delegate;
+
+		ep = usbf_get_ep_by_addr(udc, wIndex);
+		if (!ep)
+			return -EINVAL;
+
+		if ((ep->id == 0) && is_set) {
+			/* Endpoint 0 cannot be halted (stalled)
+			 * Returning an error code leads to a STALL on this ep0
+			 * but keep the automate in a consistent state.
+			 */
+			return -EINVAL;
+		}
+		if (ep->is_wedged && !is_set) {
+			/* Ignore CLEAR_FEATURE(HALT ENDPOINT) when the
+			 * endpoint is wedged
+			 */
+			break;
+		}
+		usbf_ep_stall(ep, is_set);
+		break;
+
+	default:
+		goto delegate;
+	}
+
+	return 0;
+
+delegate:
+	return usbf_req_delegate(udc, ctrlrequest);
+}
+
+static void usbf_ep0_req_set_address_complete(struct usb_ep *_ep,
+					      struct usb_request *_req)
+{
+	struct usbf_ep *ep = container_of(_ep, struct usbf_ep, ep);
+
+	/* The status phase of the SET_ADDRESS request is completed ... */
+	if (_req->status == 0) {
+		/* ... without any errors -> Signaled the state to the core. */
+		usb_gadget_set_state(&ep->udc->gadget, USB_STATE_ADDRESS);
+	}
+
+	/* In case of request failure, there is no need to revert the address
+	 * value set to the hardware as the hardware will take care of the
+	 * value only if the status stage is completed normally.
+	 */
+}
+
+static int usbf_req_set_address(struct usbf_udc *udc,
+				const struct usb_ctrlrequest *ctrlrequest)
+{
+	u16 wLength;
+	u16 wValue;
+	u16 wIndex;
+	u32 addr;
+
+	wValue  = le16_to_cpu(ctrlrequest->wValue);
+	wLength = le16_to_cpu(ctrlrequest->wLength);
+	wIndex  = le16_to_cpu(ctrlrequest->wIndex);
+
+	if (ctrlrequest->bRequestType != (USB_DIR_OUT | USB_RECIP_DEVICE))
+		goto delegate;
+
+	if ((wIndex != 0) || (wLength != 0) || (wValue > 127))
+		return -EINVAL;
+
+	addr = wValue;
+	/* The hardware will take care of this USB address after the status
+	 * stage of the SET_ADDRESS request is completed normally.
+	 * It is safe to write it now
+	 */
+	usbf_reg_writel(udc, USBF_REG_USB_ADDRESS, USBF_USB_SET_USB_ADDR(addr));
+
+	/* Queued the status request */
+	usbf_ep0_fill_req(&udc->ep[0], &udc->setup_reply, NULL, 0,
+			  usbf_ep0_req_set_address_complete);
+	usbf_ep0_queue(&udc->ep[0], &udc->setup_reply, GFP_ATOMIC);
+
+	return 0;
+
+delegate:
+	return usbf_req_delegate(udc, ctrlrequest);
+}
+
+static int usbf_req_set_configuration(struct usbf_udc *udc,
+				      const struct usb_ctrlrequest *ctrlrequest)
+{
+	u16 wLength;
+	u16 wValue;
+	u16 wIndex;
+	int ret;
+
+	ret = usbf_req_delegate(udc, ctrlrequest);
+	if (ret)
+		return ret;
+
+	wValue  = le16_to_cpu(ctrlrequest->wValue);
+	wLength = le16_to_cpu(ctrlrequest->wLength);
+	wIndex  = le16_to_cpu(ctrlrequest->wIndex);
+
+	if ((ctrlrequest->bRequestType != (USB_DIR_OUT | USB_RECIP_DEVICE)) ||
+	    (wIndex != 0) || (wLength != 0)) {
+		/* No error detected by driver->setup() but it is not an USB2.0
+		 * Ch9 SET_CONFIGURATION.
+		 * Nothing more to do
+		 */
+		return 0;
+	}
+
+	if (wValue & 0x00FF) {
+		usbf_reg_bitset(udc, USBF_REG_USB_CONTROL, USBF_USB_CONF);
+	} else {
+		usbf_reg_bitclr(udc, USBF_REG_USB_CONTROL, USBF_USB_CONF);
+		/* Go back to Address State */
+		spin_unlock(&udc->lock);
+		usb_gadget_set_state(&udc->gadget, USB_STATE_ADDRESS);
+		spin_lock(&udc->lock);
+	}
+
+	return 0;
+}
+
+static int usbf_handle_ep0_setup(struct usbf_ep *ep0)
+{
+	union {
+		struct usb_ctrlrequest ctrlreq;
+		u32 raw[2];
+	} crq;
+	struct usbf_udc *udc = ep0->udc;
+	int ret;
+
+	/* Read setup data (ie the USB control request) */
+	crq.raw[0] = usbf_reg_readl(udc, USBF_REG_SETUP_DATA0);
+	crq.raw[1] = usbf_reg_readl(udc, USBF_REG_SETUP_DATA1);
+
+	dev_dbg(ep0->udc->dev,
+		"ep0 req%02x.%02x, wValue 0x%04x, wIndex 0x%04x, wLength 0x%04x\n",
+		crq.ctrlreq.bRequestType, crq.ctrlreq.bRequest,
+		crq.ctrlreq.wValue, crq.ctrlreq.wIndex, crq.ctrlreq.wLength);
+
+	/* Set current EP0 state according to the received request */
+	if (crq.ctrlreq.wLength) {
+		if (crq.ctrlreq.bRequestType & USB_DIR_IN) {
+			udc->ep0state = EP0_IN_DATA_PHASE;
+			usbf_ep_reg_clrset(ep0, USBF_REG_EP0_CONTROL,
+				USBF_EP0_INAK,
+				USBF_EP0_INAK_EN);
+			ep0->is_in = 1;
+		} else {
+			udc->ep0state = EP0_OUT_DATA_PHASE;
+			usbf_ep_reg_bitclr(ep0, USBF_REG_EP0_CONTROL,
+				USBF_EP0_ONAK);
+			ep0->is_in = 0;
+		}
+	} else {
+		udc->ep0state = EP0_IN_STATUS_START_PHASE;
+		ep0->is_in = 1;
+	}
+
+	/* We starts a new control transfer -> Clear the delayed status flag */
+	ep0->delayed_status = 0;
+
+	if ((crq.ctrlreq.bRequestType & USB_TYPE_MASK) != USB_TYPE_STANDARD) {
+		/* This is not a USB standard request -> delelate */
+		goto delegate;
+	}
+
+	switch (crq.ctrlreq.bRequest) {
+	case USB_REQ_GET_STATUS:
+		ret = usbf_req_get_status(udc, &crq.ctrlreq);
+		break;
+
+	case USB_REQ_CLEAR_FEATURE:
+		ret = usbf_req_clear_set_feature(udc, &crq.ctrlreq, false);
+		break;
+
+	case USB_REQ_SET_FEATURE:
+		ret = usbf_req_clear_set_feature(udc, &crq.ctrlreq, true);
+		break;
+
+	case USB_REQ_SET_ADDRESS:
+		ret = usbf_req_set_address(udc, &crq.ctrlreq);
+		break;
+
+	case USB_REQ_SET_CONFIGURATION:
+		ret = usbf_req_set_configuration(udc, &crq.ctrlreq);
+		break;
+
+	default:
+		goto delegate;
+	}
+
+	return ret;
+
+delegate:
+	return usbf_req_delegate(udc, &crq.ctrlreq);
+}
+
+static int usbf_handle_ep0_data_status(struct usbf_ep *ep0,
+				  const char *ep0state_name,
+				  enum usbf_ep0state next_ep0state)
+{
+	struct usbf_udc *udc = ep0->udc;
+	int ret;
+
+	ret = usbf_ep_process_queue(ep0);
+	switch (ret) {
+	case -ENOENT:
+		dev_err(udc->dev,
+			"no request available for ep0 %s phase\n",
+			ep0state_name);
+		break;
+	case -EINPROGRESS:
+		/* More data needs to be processed */
+		ret = 0;
+		break;
+	case 0:
+		/* All requests in the queue are processed */
+		udc->ep0state = next_ep0state;
+		break;
+	default:
+		dev_err(udc->dev,
+			"process queue failed for ep0 %s phase (%d)\n",
+			ep0state_name, ret);
+		break;
+	}
+	return ret;
+}
+
+static int usbf_handle_ep0_out_status_start(struct usbf_ep *ep0)
+{
+	struct usbf_udc *udc = ep0->udc;
+	struct usbf_req *req;
+
+	usbf_ep_reg_clrset(ep0, USBF_REG_EP0_CONTROL,
+				USBF_EP0_ONAK,
+				USBF_EP0_PIDCLR);
+	ep0->is_in = 0;
+
+	req = list_first_entry_or_null(&ep0->queue, struct usbf_req, queue);
+	if (!req) {
+		usbf_ep0_fill_req(ep0, &udc->setup_reply, NULL, 0, NULL);
+		usbf_ep0_queue(ep0, &udc->setup_reply, GFP_ATOMIC);
+	} else {
+		if (req->req.length) {
+			dev_err(udc->dev,
+				"queued request length %u for ep0 out status phase\n",
+				req->req.length);
+		}
+	}
+	udc->ep0state = EP0_OUT_STATUS_PHASE;
+	return 0;
+}
+
+static int usbf_handle_ep0_in_status_start(struct usbf_ep *ep0)
+{
+	struct usbf_udc *udc = ep0->udc;
+	struct usbf_req *req;
+	int ret;
+
+	usbf_ep_reg_clrset(ep0, USBF_REG_EP0_CONTROL,
+				USBF_EP0_INAK,
+				USBF_EP0_INAK_EN | USBF_EP0_PIDCLR);
+	ep0->is_in = 1;
+
+	/* Queue request for status if needed */
+	req = list_first_entry_or_null(&ep0->queue, struct usbf_req, queue);
+	if (!req) {
+		if (ep0->delayed_status) {
+			dev_dbg(ep0->udc->dev,
+				"EP0_IN_STATUS_START_PHASE ep0->delayed_status set\n");
+			udc->ep0state = EP0_IN_STATUS_PHASE;
+			return 0;
+		}
+
+		usbf_ep0_fill_req(ep0, &udc->setup_reply, NULL,
+			  0, NULL);
+		usbf_ep0_queue(ep0, &udc->setup_reply,
+			       GFP_ATOMIC);
+
+		req = list_first_entry_or_null(&ep0->queue, struct usbf_req, queue);
+	} else {
+		if (req->req.length) {
+			dev_err(udc->dev,
+				"queued request length %u for ep0 in status phase\n",
+				req->req.length);
+		}
+	}
+
+	ret = usbf_ep0_pio_in(ep0, req);
+	if (ret != -EINPROGRESS) {
+		usbf_ep_req_done(ep0, req, ret);
+		udc->ep0state = EP0_IN_STATUS_END_PHASE;
+		return 0;
+	}
+
+	udc->ep0state = EP0_IN_STATUS_PHASE;
+	return 0;
+}
+
+static void usbf_ep0_interrupt(struct usbf_ep *ep0)
+{
+	struct usbf_udc *udc = ep0->udc;
+	u32 sts, prev_sts;
+	int prev_ep0state;
+	int ret;
+
+	ep0->status = usbf_ep_reg_readl(ep0, USBF_REG_EP0_STATUS);
+	usbf_ep_reg_writel(ep0, USBF_REG_EP0_STATUS, ~ep0->status);
+
+	dev_dbg(ep0->udc->dev, "ep0 status=0x%08x, enable=%08x\n, ctrl=0x%08x\n",
+		ep0->status,
+		usbf_ep_reg_readl(ep0, USBF_REG_EP0_INT_ENA),
+		usbf_ep_reg_readl(ep0, USBF_REG_EP0_CONTROL));
+
+	sts = ep0->status & (USBF_EP0_SETUP_INT | USBF_EP0_IN_INT | USBF_EP0_OUT_INT |
+			     USBF_EP0_OUT_NULL_INT | USBF_EP0_STG_START_INT |
+			     USBF_EP0_STG_END_INT);
+
+	ret = 0;
+	do {
+		dev_dbg(ep0->udc->dev, "udc->ep0state=%d\n", udc->ep0state);
+
+		prev_sts = sts;
+		prev_ep0state = udc->ep0state;
+		switch (udc->ep0state) {
+		case EP0_IDLE:
+			if (!(sts & USBF_EP0_SETUP_INT))
+				break;
+
+			sts &= ~USBF_EP0_SETUP_INT;
+			dev_dbg(ep0->udc->dev, "ep0 handle setup\n");
+			ret = usbf_handle_ep0_setup(ep0);
+			break;
+
+		case EP0_IN_DATA_PHASE:
+			if (!(sts & USBF_EP0_IN_INT))
+				break;
+
+			sts &= ~USBF_EP0_IN_INT;
+			dev_dbg(ep0->udc->dev, "ep0 handle in data phase\n");
+			ret = usbf_handle_ep0_data_status(ep0,
+				"in data", EP0_OUT_STATUS_START_PHASE);
+			break;
+
+		case EP0_OUT_STATUS_START_PHASE:
+			if (!(sts & USBF_EP0_STG_START_INT))
+				break;
+
+			sts &= ~USBF_EP0_STG_START_INT;
+			dev_dbg(ep0->udc->dev, "ep0 handle out status start phase\n");
+			ret = usbf_handle_ep0_out_status_start(ep0);
+			break;
+
+		case EP0_OUT_STATUS_PHASE:
+			if (!(sts & (USBF_EP0_OUT_INT | USBF_EP0_OUT_NULL_INT)))
+				break;
+
+			sts &= ~(USBF_EP0_OUT_INT | USBF_EP0_OUT_NULL_INT);
+			dev_dbg(ep0->udc->dev, "ep0 handle out status phase\n");
+			ret = usbf_handle_ep0_data_status(ep0,
+				"out status",
+				EP0_OUT_STATUS_END_PHASE);
+			break;
+
+		case EP0_OUT_STATUS_END_PHASE:
+			if (!(sts & (USBF_EP0_STG_END_INT | USBF_EP0_SETUP_INT)))
+				break;
+
+			sts &= ~USBF_EP0_STG_END_INT;
+			dev_dbg(ep0->udc->dev, "ep0 handle out status end phase\n");
+			udc->ep0state = EP0_IDLE;
+			break;
+
+		case EP0_OUT_DATA_PHASE:
+			if (!(sts & (USBF_EP0_OUT_INT | USBF_EP0_OUT_NULL_INT)))
+				break;
+
+			sts &= ~(USBF_EP0_OUT_INT | USBF_EP0_OUT_NULL_INT);
+			dev_dbg(ep0->udc->dev, "ep0 handle out data phase\n");
+			ret = usbf_handle_ep0_data_status(ep0,
+				"out data", EP0_IN_STATUS_START_PHASE);
+			break;
+
+		case EP0_IN_STATUS_START_PHASE:
+			if (!(sts & USBF_EP0_STG_START_INT))
+				break;
+
+			sts &= ~USBF_EP0_STG_START_INT;
+			dev_dbg(ep0->udc->dev, "ep0 handle in status start phase\n");
+			ret = usbf_handle_ep0_in_status_start(ep0);
+			break;
+
+		case EP0_IN_STATUS_PHASE:
+			if (!(sts & USBF_EP0_IN_INT))
+				break;
+
+			sts &= ~USBF_EP0_IN_INT;
+			dev_dbg(ep0->udc->dev, "ep0 handle in status phase\n");
+			ret = usbf_handle_ep0_data_status(ep0,
+				"in status", EP0_IN_STATUS_END_PHASE);
+			break;
+
+		case EP0_IN_STATUS_END_PHASE:
+			if (!(sts & (USBF_EP0_STG_END_INT | USBF_EP0_SETUP_INT)))
+				break;
+
+			sts &= ~USBF_EP0_STG_END_INT;
+			dev_dbg(ep0->udc->dev, "ep0 handle in status end\n");
+			udc->ep0state = EP0_IDLE;
+			break;
+
+		default:
+			udc->ep0state = EP0_IDLE;
+			break;
+		}
+
+		if (ret) {
+			dev_dbg(ep0->udc->dev, "ep0 failed (%d)\n", ret);
+			/* Failure -> stall.
+			 * This stall state will be automatically cleared when
+			 * the IP receives the next SETUP packet
+			 */
+			usbf_ep_stall(ep0, true);
+
+			/* Remove anything that was pending */
+			usbf_ep_nuke(ep0, -EPROTO);
+
+			udc->ep0state = EP0_IDLE;
+			break;
+		}
+
+	} while ((prev_ep0state != udc->ep0state) || (prev_sts != sts));
+
+	dev_dbg(ep0->udc->dev, "ep0 done udc->ep0state=%d, status=0x%08x. next=0x%08x\n",
+		udc->ep0state, sts,
+		usbf_ep_reg_readl(ep0, USBF_REG_EP0_STATUS));
+}
+
+static void usbf_epn_process_queue(struct usbf_ep *epn)
+{
+	int ret;
+
+	ret = usbf_ep_process_queue(epn);
+	switch (ret) {
+	case -ENOENT:
+		dev_warn(epn->udc->dev, "ep%u %s, no request available\n",
+			epn->id, epn->is_in ? "in" : "out");
+		break;
+	case -EINPROGRESS:
+		/* More data needs to be processed */
+		ret = 0;
+		break;
+	case 0:
+		/* All requests in the queue are processed */
+		break;
+	default:
+		dev_err(epn->udc->dev, "ep%u %s, process queue failed (%d)\n",
+			epn->id, epn->is_in ? "in" : "out", ret);
+		break;
+	}
+
+	if (ret) {
+		dev_dbg(epn->udc->dev, "ep%u %s failed (%d)\n", epn->id,
+			epn->is_in ? "in" : "out", ret);
+		usbf_ep_stall(epn, true);
+		usbf_ep_nuke(epn, ret);
+	}
+}
+
+static void usbf_epn_interrupt(struct usbf_ep *epn)
+{
+	u32 sts;
+	u32 ena;
+
+	epn->status = usbf_ep_reg_readl(epn, USBF_REG_EPN_STATUS);
+	ena = usbf_ep_reg_readl(epn, USBF_REG_EPN_INT_ENA);
+	usbf_ep_reg_writel(epn, USBF_REG_EPN_STATUS, ~(epn->status & ena));
+
+	dev_dbg(epn->udc->dev, "ep%u %s status=0x%08x, enable=%08x\n, ctrl=0x%08x\n",
+		epn->id, epn->is_in ? "in" : "out", epn->status, ena,
+		usbf_ep_reg_readl(epn, USBF_REG_EPN_CONTROL));
+
+	if (epn->disabled) {
+		dev_warn(epn->udc->dev, "ep%u %s, interrupt while disabled\n",
+			epn->id, epn->is_in ? "in" : "out");
+		return;
+	}
+
+	sts = epn->status & ena;
+
+	if (sts & (USBF_EPN_IN_END_INT | USBF_EPN_IN_INT)) {
+		sts &= ~(USBF_EPN_IN_END_INT | USBF_EPN_IN_INT);
+		dev_dbg(epn->udc->dev, "ep%u %s process queue (in interrupts)\n",
+			epn->id, epn->is_in ? "in" : "out");
+		usbf_epn_process_queue(epn);
+	}
+
+	if (sts & (USBF_EPN_OUT_END_INT | USBF_EPN_OUT_INT | USBF_EPN_OUT_NULL_INT)) {
+		sts &= ~(USBF_EPN_OUT_END_INT | USBF_EPN_OUT_INT | USBF_EPN_OUT_NULL_INT);
+		dev_dbg(epn->udc->dev, "ep%u %s process queue (out interrupts)\n",
+			epn->id, epn->is_in ? "in" : "out");
+		usbf_epn_process_queue(epn);
+	}
+
+	dev_dbg(epn->udc->dev, "ep%u %s done status=0x%08x. next=0x%08x\n",
+		epn->id, epn->is_in ? "in" : "out",
+		sts, usbf_ep_reg_readl(epn, USBF_REG_EPN_STATUS));
+}
+
+static void usbf_ep_reset(struct usbf_ep *ep)
+{
+	ep->status = 0;
+	/* Remove anything that was pending */
+	usbf_ep_nuke(ep, -ESHUTDOWN);
+}
+
+static void usbf_reset(struct usbf_udc *udc)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(udc->ep); i++) {
+		if (udc->ep[i].disabled)
+			continue;
+
+		usbf_ep_reset(&udc->ep[i]);
+	}
+
+	if (usbf_reg_readl(udc, USBF_REG_USB_STATUS) & USBF_USB_SPEED_MODE)
+		udc->gadget.speed = USB_SPEED_HIGH;
+	else
+		udc->gadget.speed = USB_SPEED_FULL;
+
+	/* Remote wakeup feature must be disabled on USB bus reset */
+	udc->is_remote_wakeup = false;
+
+	/* Enable endpoint zero */
+	usbf_ep0_enable(&udc->ep[0]);
+
+	if (udc->driver) {
+		/* Signal the reset */
+		spin_unlock(&udc->lock);
+		usb_gadget_udc_reset(&udc->gadget, udc->driver);
+		spin_lock(&udc->lock);
+	}
+}
+
+static void usbf_driver_suspend(struct usbf_udc *udc)
+{
+	if (udc->is_usb_suspended) {
+		dev_dbg(udc->dev, "already suspended\n");
+		return;
+	}
+
+	dev_dbg(udc->dev, "do usb suspend\n");
+	udc->is_usb_suspended = true;
+
+	if (udc->driver && udc->driver->suspend) {
+		spin_unlock(&udc->lock);
+		udc->driver->suspend(&udc->gadget);
+		spin_lock(&udc->lock);
+
+		/* The datasheet tells to set the USB_CONTROL register SUSPEND
+		 * bit when the USB bus suspend is detected.
+		 * This bit stops the clocks (clocks for EPC, SIE, USBPHY) but
+		 * these clocks seems not used only by the USB device. Some
+		 * UARTs can be lost ...
+		 * So, do not set the USB_CONTROL register SUSPEND bit.
+		 */
+	}
+}
+
+static void usbf_driver_resume(struct usbf_udc *udc)
+{
+	if (!udc->is_usb_suspended)
+		return;
+
+	dev_dbg(udc->dev, "do usb resume\n");
+	udc->is_usb_suspended = false;
+
+	if (udc->driver && udc->driver->resume) {
+		spin_unlock(&udc->lock);
+		udc->driver->resume(&udc->gadget);
+		spin_lock(&udc->lock);
+	}
+}
+
+static irqreturn_t usbf_epc_irq(int irq, void *_udc)
+{
+	struct usbf_udc *udc = (struct usbf_udc *)_udc;
+	unsigned long flags;
+	struct usbf_ep *ep;
+	u32 int_sts;
+	u32 int_en;
+	int i;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	int_en = usbf_reg_readl(udc, USBF_REG_USB_INT_ENA);
+	int_sts = usbf_reg_readl(udc, USBF_REG_USB_INT_STA) & int_en;
+	usbf_reg_writel(udc, USBF_REG_USB_INT_STA, ~int_sts);
+
+	dev_dbg(udc->dev, "int_sts=0x%08x\n", int_sts);
+
+	if (int_sts & USBF_USB_RSUM_INT) {
+		dev_dbg(udc->dev, "handle resume\n");
+		usbf_driver_resume(udc);
+	}
+
+	if (int_sts & USBF_USB_USB_RST_INT) {
+		dev_dbg(udc->dev, "handle bus reset\n");
+		usbf_driver_resume(udc);
+		usbf_reset(udc);
+	}
+
+	if (int_sts & USBF_USB_SPEED_MODE_INT) {
+		if (usbf_reg_readl(udc, USBF_REG_USB_STATUS) & USBF_USB_SPEED_MODE)
+			udc->gadget.speed = USB_SPEED_HIGH;
+		else
+			udc->gadget.speed = USB_SPEED_FULL;
+		dev_dbg(udc->dev, "handle speed change (%s)\n",
+			udc->gadget.speed == USB_SPEED_HIGH ? "High" : "Full");
+	}
+
+	if (int_sts & USBF_USB_EPN_INT(0)) {
+		usbf_driver_resume(udc);
+		usbf_ep0_interrupt(&udc->ep[0]);
+	}
+
+	for (i = 1; i < ARRAY_SIZE(udc->ep); i++) {
+		ep = &udc->ep[i];
+
+		if (int_sts & USBF_USB_EPN_INT(i)) {
+			usbf_driver_resume(udc);
+			usbf_epn_interrupt(ep);
+		}
+	}
+
+	if (int_sts & USBF_USB_SPND_INT) {
+		dev_dbg(udc->dev, "handle suspend\n");
+		usbf_driver_suspend(udc);
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t usbf_ahb_epc_irq(int irq, void *_udc)
+{
+	struct usbf_udc *udc = (struct usbf_udc *)_udc;
+	unsigned long flags;
+	struct usbf_ep *epn;
+	u32 sysbint;
+	void (*ep_action)(struct usbf_ep *epn);
+	int i;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* Read and ack interrupts */
+	sysbint = usbf_reg_readl(udc, USBF_REG_AHBBINT);
+	usbf_reg_writel(udc, USBF_REG_AHBBINT, sysbint);
+
+	if ((sysbint & USBF_SYS_VBUS_INT) == USBF_SYS_VBUS_INT) {
+		if (usbf_reg_readl(udc, USBF_REG_EPCTR) & USBF_SYS_VBUS_LEVEL) {
+			dev_dbg(udc->dev, "handle vbus (1)\n");
+			spin_unlock(&udc->lock);
+			usb_udc_vbus_handler(&udc->gadget, true);
+			usb_gadget_set_state(&udc->gadget, USB_STATE_POWERED);
+			spin_lock(&udc->lock);
+		} else {
+			dev_dbg(udc->dev, "handle vbus (0)\n");
+			udc->is_usb_suspended = false;
+			spin_unlock(&udc->lock);
+			usb_udc_vbus_handler(&udc->gadget, false);
+			usb_gadget_set_state(&udc->gadget,
+					     USB_STATE_NOTATTACHED);
+			spin_lock(&udc->lock);
+		}
+	}
+
+	for (i = 1; i < ARRAY_SIZE(udc->ep); i++) {
+		if (sysbint & USBF_SYS_DMA_ENDINT_EPN(i)) {
+			epn = &udc->ep[i];
+			dev_dbg(epn->udc->dev,
+				"ep%u handle DMA complete. action=%ps\n",
+				epn->id, epn->bridge_on_dma_end);
+			ep_action = epn->bridge_on_dma_end;
+			if (ep_action) {
+				epn->bridge_on_dma_end = NULL;
+				ep_action(epn);
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static int usbf_udc_start(struct usb_gadget *gadget,
+			  struct usb_gadget_driver *driver)
+{
+	struct usbf_udc *udc = container_of(gadget, struct usbf_udc, gadget);
+	unsigned long flags;
+
+	dev_info(udc->dev, "start (driver '%s')\n", driver->driver.name);
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* hook up the driver */
+	udc->driver = driver;
+
+	/* Enable VBUS interrupt */
+	usbf_reg_writel(udc, USBF_REG_AHBBINTEN, USBF_SYS_VBUS_INTEN);
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+static int usbf_udc_stop(struct usb_gadget *gadget)
+{
+	struct usbf_udc *udc = container_of(gadget, struct usbf_udc, gadget);
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* Disable VBUS interrupt */
+	usbf_reg_writel(udc, USBF_REG_AHBBINTEN, 0);
+
+	udc->driver = NULL;
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	dev_info(udc->dev, "stopped\n");
+
+	return 0;
+}
+
+static int usbf_get_frame(struct usb_gadget *gadget)
+{
+	struct usbf_udc *udc = container_of(gadget, struct usbf_udc, gadget);
+
+	return USBF_USB_GET_FRAME(usbf_reg_readl(udc, USBF_REG_USB_ADDRESS));
+}
+
+static void usbf_attach(struct usbf_udc *udc)
+{
+	/* Enable USB signal to Function PHY
+	 * D+ signal Pull-up
+	 * Disable endpoint 0, it will be automatically enable when a USB reset
+	 * is received.
+	 * Disable the other endpoints
+	 */
+	usbf_reg_clrset(udc, USBF_REG_USB_CONTROL,
+		USBF_USB_CONNECTB | USBF_USB_DEFAULT | USBF_USB_CONF,
+		USBF_USB_PUE2);
+
+	/* Enable reset and mode change interrupts */
+	usbf_reg_bitset(udc, USBF_REG_USB_INT_ENA,
+		USBF_USB_USB_RST_EN | USBF_USB_SPEED_MODE_EN | USBF_USB_RSUM_EN | USBF_USB_SPND_EN);
+}
+
+static void usbf_detach(struct usbf_udc *udc)
+{
+	int i;
+
+	/* Disable interrupts */
+	usbf_reg_writel(udc, USBF_REG_USB_INT_ENA, 0);
+
+	for (i = 0; i < ARRAY_SIZE(udc->ep); i++) {
+		if (udc->ep[i].disabled)
+			continue;
+
+		usbf_ep_reset(&udc->ep[i]);
+	}
+
+	/* Disable USB signal to Function PHY
+	 * Do not Pull-up D+ signal
+	 * Disable endpoint 0
+	 * Disable the other endpoints
+	 */
+	usbf_reg_clrset(udc, USBF_REG_USB_CONTROL,
+		USBF_USB_PUE2 | USBF_USB_DEFAULT | USBF_USB_CONF,
+		USBF_USB_CONNECTB);
+}
+
+static int usbf_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct usbf_udc *udc = container_of(gadget, struct usbf_udc, gadget);
+	unsigned long flags;
+
+	dev_dbg(udc->dev, "pullup %d\n", is_on);
+
+	spin_lock_irqsave(&udc->lock, flags);
+	if (is_on)
+		usbf_attach(udc);
+	else
+		usbf_detach(udc);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+static int usbf_udc_set_selfpowered(struct usb_gadget *gadget,
+				    int is_selfpowered)
+{
+	struct usbf_udc *udc = container_of(gadget, struct usbf_udc, gadget);
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	gadget->is_selfpowered = (is_selfpowered != 0);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+static int usbf_udc_wakeup(struct usb_gadget *gadget)
+{
+	struct usbf_udc *udc = container_of(gadget, struct usbf_udc, gadget);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	if (!udc->is_remote_wakeup) {
+		dev_dbg(udc->dev, "remote wakeup not allowed\n");
+		ret = -EINVAL;
+		goto end;
+	}
+
+	dev_dbg(udc->dev, "do wakeup\n");
+
+	/* Send the resume signal */
+	usbf_reg_bitset(udc, USBF_REG_USB_CONTROL, USBF_USB_RSUM_IN);
+	usbf_reg_bitclr(udc, USBF_REG_USB_CONTROL, USBF_USB_RSUM_IN);
+
+	ret = 0;
+end:
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return ret;
+}
+
+static struct usb_gadget_ops usbf_gadget_ops = {
+	.get_frame = usbf_get_frame,
+	.pullup = usbf_pullup,
+	.udc_start = usbf_udc_start,
+	.udc_stop = usbf_udc_stop,
+	.set_selfpowered = usbf_udc_set_selfpowered,
+	.wakeup = usbf_udc_wakeup,
+};
+
+static int usbf_epn_check(struct usbf_ep *epn)
+{
+	const char *type_txt;
+	const char *buf_txt;
+	int ret = 0;
+	u32 ctrl;
+
+	ctrl = usbf_ep_reg_readl(epn, USBF_REG_EPN_CONTROL);
+
+	switch (ctrl & USBF_EPN_MODE_MASK) {
+	case USBF_EPN_MODE_BULK:
+		type_txt = "bulk";
+		if (epn->ep.caps.type_control || epn->ep.caps.type_iso ||
+		    !epn->ep.caps.type_bulk || epn->ep.caps.type_int) {
+			dev_err(epn->udc->dev,
+				"ep%u caps mismatch, bulk expected\n", epn->id);
+			ret = -EINVAL;
+		}
+		break;
+	case USBF_EPN_MODE_INTR:
+		type_txt = "intr";
+		if (epn->ep.caps.type_control || epn->ep.caps.type_iso ||
+		    epn->ep.caps.type_bulk || !epn->ep.caps.type_int) {
+			dev_err(epn->udc->dev,
+				"ep%u caps mismatch, int expected\n", epn->id);
+			ret = -EINVAL;
+		}
+		break;
+	case USBF_EPN_MODE_ISO:
+		type_txt = "iso";
+		if (epn->ep.caps.type_control || !epn->ep.caps.type_iso ||
+		    epn->ep.caps.type_bulk || epn->ep.caps.type_int) {
+			dev_err(epn->udc->dev,
+				"ep%u caps mismatch, iso expected\n", epn->id);
+			ret = -EINVAL;
+		}
+		break;
+	default:
+		type_txt = "unknown";
+		dev_err(epn->udc->dev, "ep%u unknown type\n", epn->id);
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ctrl & USBF_EPN_BUF_TYPE_DOUBLE) {
+		buf_txt = "double";
+		if (!usbf_ep_info[epn->id].is_double) {
+			dev_err(epn->udc->dev,
+				"ep%u buffer mismatch, double expected\n",
+				epn->id);
+			ret = -EINVAL;
+		}
+	} else {
+		buf_txt = "single";
+		if (usbf_ep_info[epn->id].is_double) {
+			dev_err(epn->udc->dev,
+				"ep%u buffer mismatch, single expected\n",
+				epn->id);
+			ret = -EINVAL;
+		}
+	}
+
+	dev_dbg(epn->udc->dev, "ep%u (%s) %s, %s buffer %u, checked %s\n",
+		 epn->id, epn->ep.name, type_txt, buf_txt,
+		 epn->ep.maxpacket_limit, ret ? "failed" : "ok");
+
+	return ret;
+}
+
+static int usbf_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct usbf_udc *udc;
+	struct usbf_ep *ep;
+	unsigned int i;
+	int irq;
+	int ret;
+
+	udc = devm_kzalloc(dev, sizeof(*udc), GFP_KERNEL);
+	if (!udc)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, udc);
+
+	udc->dev = dev;
+	spin_lock_init(&udc->lock);
+
+	udc->regs = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(udc->regs))
+		return PTR_ERR(udc->regs);
+
+	devm_pm_runtime_enable(&pdev->dev);
+	ret = pm_runtime_resume_and_get(&pdev->dev);
+	if (ret < 0)
+		return ret;
+
+	dev_info(dev, "USBF version: %08x\n",
+		usbf_reg_readl(udc, USBF_REG_USBSSVER));
+
+	/* Resetting the PLL is handled via the clock driver as it has common
+	 * registers with USB Host
+	 */
+	usbf_reg_bitclr(udc, USBF_REG_EPCTR, USBF_SYS_EPC_RST);
+
+	/* modify in register gadget process */
+	udc->gadget.speed = USB_SPEED_FULL;
+	udc->gadget.max_speed = USB_SPEED_HIGH;
+	udc->gadget.ops = &usbf_gadget_ops;
+
+	udc->gadget.name = dev->driver->name;
+	udc->gadget.dev.parent = dev;
+	udc->gadget.ep0 = &udc->ep[0].ep;
+
+	/* The hardware DMA controller needs dma addresses aligned on 32bit.
+	 * A fallback to pio is done if DMA addresses are not aligned.
+	 */
+	udc->gadget.quirk_avoids_skb_reserve = 1;
+
+	INIT_LIST_HEAD(&udc->gadget.ep_list);
+	/* we have a canned request structure to allow sending packets as reply
+	 * to get_status requests
+	 */
+	INIT_LIST_HEAD(&udc->setup_reply.queue);
+
+	for (i = 0; i < ARRAY_SIZE(udc->ep); i++) {
+		ep = &udc->ep[i];
+
+		if (!(usbf_reg_readl(udc, USBF_REG_USBSSCONF) &
+		      USBF_SYS_EP_AVAILABLE(i))) {
+			continue;
+		}
+
+		INIT_LIST_HEAD(&ep->queue);
+
+		ep->id = i;
+		ep->disabled = 1;
+		ep->udc = udc;
+		ep->ep.ops = &usbf_ep_ops;
+		ep->ep.name = usbf_ep_info[i].name;
+		ep->ep.caps = usbf_ep_info[i].caps;
+		usb_ep_set_maxpacket_limit(&ep->ep,
+					   usbf_ep_info[i].maxpacket_limit);
+
+		if (ep->id == 0) {
+			ep->regs = ep->udc->regs + USBF_BASE_EP0;
+		} else {
+			ep->regs = ep->udc->regs + USBF_BASE_EPN(ep->id - 1);
+			ret = usbf_epn_check(ep);
+			if (ret)
+				return ret;
+			if (usbf_reg_readl(udc, USBF_REG_USBSSCONF) &
+			    USBF_SYS_DMA_AVAILABLE(i)) {
+				ep->dma_regs = ep->udc->regs +
+					       USBF_BASE_DMA_EPN(ep->id - 1);
+			}
+			list_add_tail(&ep->ep.ep_list, &udc->gadget.ep_list);
+		}
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+	ret = devm_request_irq(dev, irq, usbf_epc_irq, 0, "usbf-epc", udc);
+	if (ret) {
+		dev_err(dev, "cannot request irq %d err %d\n", irq, ret);
+		return ret;
+	}
+
+	irq = platform_get_irq(pdev, 1);
+	if (irq < 0)
+		return irq;
+	ret = devm_request_irq(dev, irq, usbf_ahb_epc_irq, 0, "usbf-ahb-epc", udc);
+	if (ret) {
+		dev_err(dev, "cannot request irq %d err %d\n", irq, ret);
+		return ret;
+	}
+
+	usbf_reg_bitset(udc, USBF_REG_AHBMCTR, USBF_SYS_WBURST_TYPE);
+
+	usbf_reg_bitset(udc, USBF_REG_USB_CONTROL,
+		USBF_USB_INT_SEL | USBF_USB_SOF_RCV | USBF_USB_SOF_CLK_MODE);
+
+	ret = usb_add_gadget_udc(dev, &udc->gadget);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int usbf_remove(struct platform_device *pdev)
+{
+	struct usbf_udc *udc = platform_get_drvdata(pdev);
+
+	usb_del_gadget_udc(&udc->gadget);
+
+	pm_runtime_put(&pdev->dev);
+
+	return 0;
+}
+
+static const struct of_device_id usbf_match[] = {
+	{ .compatible = "renesas,rzn1-usbf" },
+	{} /* sentinel */
+};
+MODULE_DEVICE_TABLE(of, usbf_match);
+
+static struct platform_driver udc_driver = {
+	.driver = {
+		.name = "usbf_renesas",
+		.owner = THIS_MODULE,
+		.of_match_table = usbf_match,
+	},
+	.probe          = usbf_probe,
+	.remove         = usbf_remove,
+};
+
+module_platform_driver(udc_driver);
+
+MODULE_AUTHOR("Herve Codina <herve.codina@bootlin.com>");
+MODULE_DESCRIPTION("Renesas R-Car Gen3 & RZ/N1 USB Function driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/gadget/udc/rzv2m_usb3drd.c b/drivers/usb/gadget/udc/rzv2m_usb3drd.c
new file mode 100644
index 000000000000..3c8bbf843038
--- /dev/null
+++ b/drivers/usb/gadget/udc/rzv2m_usb3drd.c
@@ -0,0 +1,139 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Renesas RZ/V2M USB3DRD driver
+ *
+ * Copyright (C) 2022 Renesas Electronics Corporation
+ */
+
+#include <linux/io.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset.h>
+#include <linux/usb/rzv2m_usb3drd.h>
+
+#define USB_PERI_DRD_CON	0x000
+
+#define USB_PERI_DRD_CON_PERI_RST	BIT(31)
+#define USB_PERI_DRD_CON_HOST_RST	BIT(30)
+#define USB_PERI_DRD_CON_PERI_CON	BIT(24)
+
+static void rzv2m_usb3drd_set_bit(struct rzv2m_usb3drd *usb3, u32 bits,
+				  u32 offs)
+{
+	u32 val = readl(usb3->reg + offs);
+
+	val |= bits;
+	writel(val, usb3->reg + offs);
+}
+
+static void rzv2m_usb3drd_clear_bit(struct rzv2m_usb3drd *usb3, u32 bits,
+				    u32 offs)
+{
+	u32 val = readl(usb3->reg + offs);
+
+	val &= ~bits;
+	writel(val, usb3->reg + offs);
+}
+
+void rzv2m_usb3drd_reset(struct device *dev, bool host)
+{
+	struct rzv2m_usb3drd *usb3 = dev_get_drvdata(dev);
+
+	if (host) {
+		rzv2m_usb3drd_clear_bit(usb3, USB_PERI_DRD_CON_PERI_CON,
+					USB_PERI_DRD_CON);
+		rzv2m_usb3drd_clear_bit(usb3, USB_PERI_DRD_CON_HOST_RST,
+					USB_PERI_DRD_CON);
+		rzv2m_usb3drd_set_bit(usb3, USB_PERI_DRD_CON_PERI_RST,
+				      USB_PERI_DRD_CON);
+	} else {
+		rzv2m_usb3drd_set_bit(usb3, USB_PERI_DRD_CON_PERI_CON,
+				      USB_PERI_DRD_CON);
+		rzv2m_usb3drd_set_bit(usb3, USB_PERI_DRD_CON_HOST_RST,
+				      USB_PERI_DRD_CON);
+		rzv2m_usb3drd_clear_bit(usb3, USB_PERI_DRD_CON_PERI_RST,
+					USB_PERI_DRD_CON);
+	}
+}
+EXPORT_SYMBOL_GPL(rzv2m_usb3drd_reset);
+
+static int rzv2m_usb3drd_remove(struct platform_device *pdev)
+{
+	struct rzv2m_usb3drd *usb3 = platform_get_drvdata(pdev);
+
+	of_platform_depopulate(usb3->dev);
+	pm_runtime_put(usb3->dev);
+	pm_runtime_disable(&pdev->dev);
+	reset_control_assert(usb3->drd_rstc);
+
+	return 0;
+}
+
+static int rzv2m_usb3drd_probe(struct platform_device *pdev)
+{
+	struct rzv2m_usb3drd *usb3;
+	int ret;
+
+	usb3 = devm_kzalloc(&pdev->dev, sizeof(*usb3), GFP_KERNEL);
+	if (!usb3)
+		return -ENOMEM;
+
+	usb3->dev = &pdev->dev;
+
+	usb3->drd_irq = platform_get_irq_byname(pdev, "drd");
+	if (usb3->drd_irq < 0)
+		return usb3->drd_irq;
+
+	usb3->reg = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(usb3->reg))
+		return PTR_ERR(usb3->reg);
+
+	platform_set_drvdata(pdev, usb3);
+
+	usb3->drd_rstc = devm_reset_control_get_exclusive(&pdev->dev, NULL);
+	if (IS_ERR(usb3->drd_rstc))
+		return dev_err_probe(&pdev->dev, PTR_ERR(usb3->drd_rstc),
+				     "failed to get drd reset");
+
+	reset_control_deassert(usb3->drd_rstc);
+	pm_runtime_enable(&pdev->dev);
+	ret = pm_runtime_resume_and_get(usb3->dev);
+	if (ret)
+		goto err_rst;
+
+	ret = of_platform_populate(usb3->dev->of_node, NULL, NULL, usb3->dev);
+	if (ret)
+		goto err_pm;
+
+	return 0;
+
+err_pm:
+	pm_runtime_put(usb3->dev);
+
+err_rst:
+	pm_runtime_disable(&pdev->dev);
+	reset_control_assert(usb3->drd_rstc);
+	return ret;
+}
+
+static const struct of_device_id rzv2m_usb3drd_of_match[] = {
+	{ .compatible = "renesas,rzv2m-usb3drd", },
+	{ /* Sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, rzv2m_usb3drd_of_match);
+
+static struct platform_driver rzv2m_usb3drd_driver = {
+	.driver = {
+		.name = "rzv2m-usb3drd",
+		.of_match_table = of_match_ptr(rzv2m_usb3drd_of_match),
+	},
+	.probe = rzv2m_usb3drd_probe,
+	.remove = rzv2m_usb3drd_remove,
+};
+module_platform_driver(rzv2m_usb3drd_driver);
+
+MODULE_AUTHOR("Biju Das <biju.das.jz@bp.renesas.com>");
+MODULE_DESCRIPTION("Renesas RZ/V2M USB3DRD driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rzv2m_usb3drd");
diff --git a/drivers/usb/gadget/udc/tegra-xudc.c b/drivers/usb/gadget/udc/tegra-xudc.c
index 76919d7570d2..2b71b33725f1 100644
--- a/drivers/usb/gadget/udc/tegra-xudc.c
+++ b/drivers/usb/gadget/udc/tegra-xudc.c
@@ -796,21 +796,16 @@ static int tegra_xudc_get_phy_index(struct tegra_xudc *xudc,
 	return -1;
 }
 
-static int tegra_xudc_vbus_notify(struct notifier_block *nb,
-					 unsigned long action, void *data)
+static void tegra_xudc_update_data_role(struct tegra_xudc *xudc,
+					      struct usb_phy *usbphy)
 {
-	struct tegra_xudc *xudc = container_of(nb, struct tegra_xudc,
-					       vbus_nb);
-	struct usb_phy *usbphy = (struct usb_phy *)data;
 	int phy_index;
 
-	dev_dbg(xudc->dev, "%s(): event is %d\n", __func__, usbphy->last_event);
-
 	if ((xudc->device_mode && usbphy->last_event == USB_EVENT_VBUS) ||
 	    (!xudc->device_mode && usbphy->last_event != USB_EVENT_VBUS)) {
 		dev_dbg(xudc->dev, "Same role(%d) received. Ignore",
 			xudc->device_mode);
-		return NOTIFY_OK;
+		return;
 	}
 
 	xudc->device_mode = (usbphy->last_event == USB_EVENT_VBUS) ? true :
@@ -826,6 +821,18 @@ static int tegra_xudc_vbus_notify(struct notifier_block *nb,
 		xudc->curr_usbphy = usbphy;
 		schedule_work(&xudc->usb_role_sw_work);
 	}
+}
+
+static int tegra_xudc_vbus_notify(struct notifier_block *nb,
+					 unsigned long action, void *data)
+{
+	struct tegra_xudc *xudc = container_of(nb, struct tegra_xudc,
+					       vbus_nb);
+	struct usb_phy *usbphy = (struct usb_phy *)data;
+
+	dev_dbg(xudc->dev, "%s(): event is %d\n", __func__, usbphy->last_event);
+
+	tegra_xudc_update_data_role(xudc, usbphy);
 
 	return NOTIFY_OK;
 }
@@ -3521,7 +3528,7 @@ static int tegra_xudc_phy_get(struct tegra_xudc *xudc)
 			/* Get usb-phy, if utmi phy is available */
 			xudc->usbphy[i] = devm_usb_get_phy_by_node(xudc->dev,
 						xudc->utmi_phy[i]->dev.of_node,
-						&xudc->vbus_nb);
+						NULL);
 			if (IS_ERR(xudc->usbphy[i])) {
 				err = PTR_ERR(xudc->usbphy[i]);
 				dev_err_probe(xudc->dev, err,
@@ -3660,6 +3667,19 @@ static struct tegra_xudc_soc tegra194_xudc_soc_data = {
 	.has_ipfs = false,
 };
 
+static struct tegra_xudc_soc tegra234_xudc_soc_data = {
+	.clock_names = tegra186_xudc_clock_names,
+	.num_clks = ARRAY_SIZE(tegra186_xudc_clock_names),
+	.num_phys = 4,
+	.u1_enable = true,
+	.u2_enable = true,
+	.lpm_enable = true,
+	.invalid_seq_num = false,
+	.pls_quirk = false,
+	.port_reset_quirk = false,
+	.has_ipfs = false,
+};
+
 static const struct of_device_id tegra_xudc_of_match[] = {
 	{
 		.compatible = "nvidia,tegra210-xudc",
@@ -3673,6 +3693,10 @@ static const struct of_device_id tegra_xudc_of_match[] = {
 		.compatible = "nvidia,tegra194-xudc",
 		.data = &tegra194_xudc_soc_data
 	},
+	{
+		.compatible = "nvidia,tegra234-xudc",
+		.data = &tegra234_xudc_soc_data
+	},
 	{ }
 };
 MODULE_DEVICE_TABLE(of, tegra_xudc_of_match);
@@ -3856,6 +3880,14 @@ static int tegra_xudc_probe(struct platform_device *pdev)
 		goto free_eps;
 	}
 
+	for (i = 0; i < xudc->soc->num_phys; i++) {
+		if (!xudc->usbphy[i])
+			continue;
+
+		usb_register_notifier(xudc->usbphy[i], &xudc->vbus_nb);
+		tegra_xudc_update_data_role(xudc, xudc->usbphy[i]);
+	}
+
 	return 0;
 
 free_eps: