[PATCH 29/31] usb: usbssp: added support for LPM.

[Pawel Laszczak <pawell@cadence.com>]

Patch implements LPM functionality for port working in HS mode.

Signed-off-by: Pawel Laszczak <pawell@cadence.com>
---
 drivers/usb/usbssp/gadget-port.c |  2 +-
 drivers/usb/usbssp/gadget-ring.c | 11 +++++++
 drivers/usb/usbssp/gadget.c      | 49 ++++++++++++++++++++++++++++++++
 drivers/usb/usbssp/gadget.h      |  4 +++
 4 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/drivers/usb/usbssp/gadget-port.c b/drivers/usb/usbssp/gadget-port.c
index 2c4d28070cab..05a4eb2fd8bf 100644
--- a/drivers/usb/usbssp/gadget-port.c
+++ b/drivers/usb/usbssp/gadget-port.c
@@ -10,9 +10,9 @@
  * Origin: Copyright (C) 2008 Intel Corp
  */
 
+#include <linux/pm_runtime.h>
 #include <linux/slab.h>
 #include <asm/unaligned.h>
-
 #include "gadget-trace.h"
 #include "gadget.h"
 
diff --git a/drivers/usb/usbssp/gadget-ring.c b/drivers/usb/usbssp/gadget-ring.c
index e7afd185d4ad..4abc2293f249 100644
--- a/drivers/usb/usbssp/gadget-ring.c
+++ b/drivers/usb/usbssp/gadget-ring.c
@@ -3018,6 +3018,17 @@ int usbssp_queue_ctrl_tx(struct usbssp_udc *usbssp_data,
 				USB_STATE_CONFIGURED);
 	}
 
+	if (usbssp_data->bos_event_detected) {
+		usbssp_data->bos_event_detected = 0;
+		usb_gadget_unmap_request_by_dev(usbssp_data->dev,
+						&req_priv->request,
+						dep->direction);
+		usbssp_set_usb2_hardware_lpm(usbssp_data,
+					&req_priv->request, 1);
+		ret = usb_gadget_map_request_by_dev(usbssp_data->dev,
+						&req_priv->request, dep->direction);
+	}
+
 	/* 1 TRB for data, 1 for status */
 	if (usbssp_data->three_stage_setup)
 		num_trbs = 2;
diff --git a/drivers/usb/usbssp/gadget.c b/drivers/usb/usbssp/gadget.c
index 378828d10a2e..029d6313d94c 100644
--- a/drivers/usb/usbssp/gadget.c
+++ b/drivers/usb/usbssp/gadget.c
@@ -1702,6 +1702,55 @@ int usbssp_enable_device(struct usbssp_udc *usbssp_data)
 	return usbssp_setup_device(usbssp_data, SETUP_CONTEXT_ONLY);
 }
 
+int usbssp_set_usb2_hardware_lpm(struct usbssp_udc *usbssp_data,
+				 struct usb_request *req, int enable)
+{
+	__le32 __iomem *pm_addr;
+	u32 pm_val, field;
+	int besl;
+
+	struct usb_ext_cap_descriptor *usb_ext = req->buf + USB_DT_BOS_SIZE;
+
+	if (usbssp_data->port_major_revision >= 3 ||
+	   !usbssp_data->hw_lpm_support ||
+	   !usbssp_data->gadget.lpm_capable)
+		return -EPERM;
+
+	if (usb_ext->bDescriptorType != USB_DT_DEVICE_CAPABILITY ||
+	    usb_ext->bDevCapabilityType != USB_CAP_TYPE_EXT) {
+		return -EPERM;
+	}
+	pm_addr = usbssp_data->usb2_ports + PORTPMSC;
+	pm_val = readl(pm_addr);
+	field = le32_to_cpu(usb_ext->bmAttributes);
+
+	dev_dbg(usbssp_data->dev, "%s port %d USB2 hardware LPM\n",
+		enable ? "enable" : "disable", usbssp_data->devs.port_num);
+
+	if (enable) {
+		/*
+		 * If device doesn't have a preferred BESL value use a
+		 * default one . See USBSSP_DEFAULT_BESL definition in gadget.h
+		 */
+		if ((field & USB_BESL_SUPPORT) &&
+		    (field & USB_BESL_BASELINE_VALID))
+			besl = USB_GET_BESL_BASELINE(field);
+		else
+			besl = USBSSP_DEFAULT_BESL;
+
+		pm_val &= ~(PORT_BESL_MASK | PORT_HLE_MASK);
+		pm_val |= PORT_RBESL(besl) | PORT_HLE | 3 /*L1S set to 3*/;
+		pr_err("usbssp_set_usb2_hardware_lpm7 %08x\n", pm_val);
+		writel(pm_val, pm_addr);
+		/* flush write */
+		readl(pm_addr);
+	} else {
+		pm_val &= ~(PORT_HLE | PORT_BESL_MASK | PORT_L1S_MASK);
+		pm_val |= PORT_L1S_HLE0_STALL;
+		writel(pm_val, pm_addr);
+	}
+	return 0;
+}
 
 int usbssp_get_frame(struct usbssp_udc *usbssp_data)
 {
diff --git a/drivers/usb/usbssp/gadget.h b/drivers/usb/usbssp/gadget.h
index 3a223b89efe6..59d7ef573d96 100644
--- a/drivers/usb/usbssp/gadget.h
+++ b/drivers/usb/usbssp/gadget.h
@@ -1756,6 +1756,10 @@ int usbssp_alloc_dev(struct usbssp_udc *usbssp_data);
 void usbssp_free_dev(struct usbssp_udc *usbssp_data);
 int usbssp_address_device(struct usbssp_udc *usbssp_data);
 int usbssp_enable_device(struct usbssp_udc *usbssp_data);
+
+int usbssp_set_usb2_hardware_lpm(struct usbssp_udc *usbsssp_data,
+				struct usb_request *req, int enable);
+
 /* USBSSP ring, segment, TRB, and TD functions */
 dma_addr_t usbssp_trb_virt_to_dma(struct usbssp_segment *seg,
 				union usbssp_trb *trb);
-- 
2.17.1