[PATCH] usb: gadget: configfs: Add lpm_Ux_disable

[Claus H. Stovgaard]

When combining dwc3 with an redriver for a USB Type-C device solution, it
sometimes have problems with leaving U1/U2 for certain hosts, resulting in
link training errors and reconnects. This create an interface via
configfs for disabling U1/U2, enabling a workaround for devices based on
dwc3.

Signed-off-by: Claus H. Stovgaard <cst@xxxxxxxxxxxx>
---
 drivers/usb/dwc3/ep0.c        |  9 ++++++-
 drivers/usb/gadget/configfs.c | 56 +++++++++++++++++++++++++++++++++++++++++++
 include/linux/usb/gadget.h    |  6 ++++-
 3 files changed, 69 insertions(+), 2 deletions(-)

diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 8efde17..5b2d26b 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -379,6 +379,8 @@ static int dwc3_ep0_handle_u1(struct dwc3 *dwc, enum usb_device_state state,
 	if ((dwc->speed != DWC3_DSTS_SUPERSPEED) &&
 			(dwc->speed != DWC3_DSTS_SUPERSPEED_PLUS))
 		return -EINVAL;
+	if (dwc->gadget_driver->lpm_U1_disable)
+		return -EINVAL;
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	if (set)
@@ -401,6 +403,8 @@ static int dwc3_ep0_handle_u2(struct dwc3 *dwc, enum usb_device_state state,
 	if ((dwc->speed != DWC3_DSTS_SUPERSPEED) &&
 			(dwc->speed != DWC3_DSTS_SUPERSPEED_PLUS))
 		return -EINVAL;
+	if (dwc->gadget_driver->lpm_U2_disable)
+		return -EINVAL;
 
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	if (set)
@@ -626,7 +630,10 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 			 * nothing is pending from application.
 			 */
 			reg = dwc3_readl(dwc->regs, DWC3_DCTL);
-			reg |= (DWC3_DCTL_ACCEPTU1ENA | DWC3_DCTL_ACCEPTU2ENA);
+			if (!dwc->gadget_driver->lpm_U1_disable)
+				reg |= DWC3_DCTL_ACCEPTU1ENA;
+			if (!dwc->gadget_driver->lpm_U2_disable)
+				reg |= DWC3_DCTL_ACCEPTU2ENA;
 			dwc3_writel(dwc->regs, DWC3_DCTL, reg);
 		}
 		break;
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index 0251299..2ee9d10 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -229,6 +229,56 @@ static ssize_t gadget_dev_desc_bcdUSB_store(struct config_item *item,
 	return len;
 }
 
+static ssize_t gadget_dev_desc_lpm_U1_disable_show(struct config_item *item,
+		char *page)
+{
+	struct gadget_info *gi = to_gadget_info(item);
+
+	return sprintf(page, "%d\n",
+		       gi->composite.gadget_driver.lpm_U1_disable);
+}
+
+static ssize_t gadget_dev_desc_lpm_U1_disable_store(struct config_item *item,
+		const char *page, size_t len)
+{
+	struct gadget_info *gi = to_gadget_info(item);
+	bool disable;
+	int ret;
+
+	ret = strtobool(page, &disable);
+	if (!ret) {
+		gi->composite.gadget_driver.lpm_U1_disable = disable;
+		ret = len;
+	}
+
+	return ret;
+}
+
+static ssize_t gadget_dev_desc_lpm_U2_disable_show(struct config_item *item,
+		char *page)
+{
+	struct gadget_info *gi = to_gadget_info(item);
+
+	return sprintf(page, "%d\n",
+		       gi->composite.gadget_driver.lpm_U2_disable);
+}
+
+static ssize_t gadget_dev_desc_lpm_U2_disable_store(struct config_item *item,
+		const char *page, size_t len)
+{
+	struct gadget_info *gi = to_gadget_info(item);
+	bool disable;
+	int ret;
+
+	ret = strtobool(page, &disable);
+	if (!ret) {
+		gi->composite.gadget_driver.lpm_U2_disable = disable;
+		ret = len;
+	}
+
+	return ret;
+}
+
 static ssize_t gadget_dev_desc_UDC_show(struct config_item *item, char *page)
 {
 	char *udc_name = to_gadget_info(item)->composite.gadget_driver.udc_name;
@@ -299,6 +349,8 @@ CONFIGFS_ATTR(gadget_dev_desc_, idVendor);
 CONFIGFS_ATTR(gadget_dev_desc_, idProduct);
 CONFIGFS_ATTR(gadget_dev_desc_, bcdDevice);
 CONFIGFS_ATTR(gadget_dev_desc_, bcdUSB);
+CONFIGFS_ATTR(gadget_dev_desc_, lpm_U1_disable);
+CONFIGFS_ATTR(gadget_dev_desc_, lpm_U2_disable);
 CONFIGFS_ATTR(gadget_dev_desc_, UDC);
 
 static struct configfs_attribute *gadget_root_attrs[] = {
@@ -310,6 +362,8 @@ static struct configfs_attribute *gadget_root_attrs[] = {
 	&gadget_dev_desc_attr_idProduct,
 	&gadget_dev_desc_attr_bcdDevice,
 	&gadget_dev_desc_attr_bcdUSB,
+	&gadget_dev_desc_attr_lpm_U1_disable,
+	&gadget_dev_desc_attr_lpm_U2_disable,
 	&gadget_dev_desc_attr_UDC,
 	NULL,
 };
@@ -1408,6 +1462,8 @@ static const struct usb_gadget_driver configfs_driver_template = {
 		.name		= "configfs-gadget",
 	},
 	.match_existing_only = 1,
+	.lpm_U1_disable = 0,
+	.lpm_U2_disable = 0,
 };
 
 static struct config_group *gadgets_make(
diff --git a/include/linux/usb/gadget.h b/include/linux/usb/gadget.h
index 7595056..25fe72b 100644
--- a/include/linux/usb/gadget.h
+++ b/include/linux/usb/gadget.h
@@ -619,7 +619,9 @@ static inline int usb_gadget_activate(struct usb_gadget *gadget)
  *	this driver will be bound to any available UDC.
  * @pending: UDC core private data used for deferred probe of this driver.
  * @match_existing_only: If udc is not found, return an error and don't add this
- *      gadget driver to list of pending driver
+ *      gadget driver to list of pending driver.
+ * @lpm_U1_disable: Instruct the UDC to disable U1 if possible.
+ * @lpm_U2_disable: Instruct the UDC to disable U2 if possible.
  *
  * Devices are disabled till a gadget driver successfully bind()s, which
  * means the driver will handle setup() requests needed to enumerate (and
@@ -684,6 +686,8 @@ struct usb_gadget_driver {
 	char			*udc_name;
 	struct list_head	pending;
 	unsigned                match_existing_only:1;
+	unsigned		lpm_U1_disable:1;
+	unsigned		lpm_U2_disable:1;
 };
 
 
-- 
2.7.4