[PATCH 9/9 v2] xHCI: set USB2 hardware LPM

[Andiry Xu <andiry.xu@xxxxxxx>]

If the device pass the USB2 software LPM and the host supports hardware
LPM, enable hardware LPM for the device to let the host decide when to
put the link into lower power state.

Signed-off-by: Andiry Xu <andiry.xu@xxxxxxx>
---
 drivers/usb/host/xhci.c |   55 ++++++++++++++++++++++++++++++++++++++++++++++-
 drivers/usb/host/xhci.h |    3 ++
 2 files changed, 57 insertions(+), 1 deletions(-)

diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index fce413a..519b1bc 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -2680,6 +2680,9 @@ command_cleanup:
 	return ret;
 }
 
+static int xhci_set_hardware_lpm(struct xhci_hcd *xhci,
+			struct usb_device *udev, int enable);
+
 /*
  * At this point, the struct usb_device is about to go away, the device has
  * disconnected, and all traffic has been stopped and the endpoints have been
@@ -2709,6 +2712,13 @@ void xhci_free_dev(struct usb_hcd *hcd, struct usb_device *udev)
 	}
 
 	spin_lock_irqsave(&xhci->lock, flags);
+
+	if (virt_dev->hw_lpm_enabled) {
+		xhci_dbg(xhci, "disable hardware lpm\n");
+		xhci_set_hardware_lpm(xhci, udev, 0);
+		virt_dev->hw_lpm_enabled = 0;
+	}
+
 	/* Don't disable the slot if the host controller is dead. */
 	state = xhci_readl(xhci, &xhci->op_regs->status);
 	if (state == 0xffffffff || (xhci->xhc_state & XHCI_STATE_DYING) ||
@@ -3105,17 +3115,60 @@ static int xhci_usb2_software_lpm_test(struct xhci_hcd *xhci,
 	return ret;
 }
 
+static int xhci_set_hardware_lpm(struct xhci_hcd *xhci,
+			struct usb_device *udev, int enable)
+{
+	__le32 __iomem	**port_array;
+	__le32 __iomem	*pm_addr;
+	u32		temp;
+	unsigned int	port_num;
+	int		u2del, hird;
+
+	port_array = xhci->usb2_ports;
+	port_num = udev->portnum - 1;
+	pm_addr = port_array[port_num] + 1;
+	temp = xhci_readl(xhci, pm_addr);
+
+	u2del = HCS_U2_LATENCY(xhci->hcs_params3);
+	if (le32_to_cpu(udev->bos->ext_cap->bmAttributes) & (1 << 2))
+		hird = xhci_calculate_hird_besl(u2del, 1);
+	else
+		hird = xhci_calculate_hird_besl(u2del, 0);
+
+	if (enable) {
+		temp &= ~PORT_HIRD_MASK;
+		temp |= PORT_HIRD(hird) | PORT_RWE;
+		xhci_writel(xhci, temp, pm_addr);
+		temp = xhci_readl(xhci, pm_addr);
+		temp |= PORT_HLE;
+		xhci_writel(xhci, temp, pm_addr);
+	} else {
+		temp &= ~(PORT_HLE | PORT_RWE | PORT_HIRD_MASK);
+		xhci_writel(xhci, temp, pm_addr);
+	}
+
+	return 0;
+}
+
 int xhci_update_device(struct usb_hcd *hcd, struct usb_device *udev)
 {
 	struct xhci_hcd	*xhci = hcd_to_xhci(hcd);
+	struct xhci_virt_device	*virt_dev;
 	unsigned long	flags;
 	int		ret;
 
 	spin_lock_irqsave(&xhci->lock, flags);
 
 	ret = xhci_usb2_software_lpm_test(xhci, udev, flags);
-	if (!ret)
+	if (!ret) {
 		xhci_dbg(xhci, "software LPM test succeed\n");
+		if (xhci->hw_lpm_support) {
+			xhci_dbg(xhci, "enable hardware LPM\n");
+			xhci_set_hardware_lpm(xhci, udev, 1);
+			virt_dev = xhci->devs[udev->slot_id];
+			virt_dev->hw_lpm_enabled = 1;
+		}
+	}
 
 	spin_unlock_irqrestore(&xhci->lock, flags);
 	return 0;
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 9a94308..f563b49 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -367,7 +367,9 @@ struct xhci_op_regs {
 #define	PORT_L1S_SUCCESS	1
 #define	PORT_RWE		(1 << 3)
 #define	PORT_HIRD(p)		(((p) & 0xf) << 4)
+#define	PORT_HIRD_MASK		(0xf << 4)
 #define	PORT_L1DS(p)		(((p) & 0xff) << 8)
+#define	PORT_HLE		(1 << 16)
 
 /**
  * struct xhci_intr_reg - Interrupt Register Set
@@ -804,6 +806,7 @@ struct xhci_virt_device {
 	u32				cmd_status;
 	struct list_head		cmd_list;
 	u8				port;
+	unsigned			hw_lpm_enabled:1;
 };
 
 
-- 
1.7.1


--
To unsubscribe from this list: send the line "unsubscribe linux-usb" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at  http://vger.kernel.org/majordomo-info.html