5 files changed, 127 insertions, 1 deletions

diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 20ca6a9ff21..baf9b648bb1 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -101,6 +101,11 @@ static int ignore_oc = 0;
 module_param (ignore_oc, bool, S_IRUGO);
 MODULE_PARM_DESC (ignore_oc, "ignore bogus hardware overcurrent indications");
 
+/* for link power management(LPM) feature */
+static unsigned int hird;
+module_param(hird, int, S_IRUGO);
+MODULE_PARM_DESC(hird, "host initiated resume duration, +1 for each 75us\n");
+
 #define	INTR_MASK (STS_IAA | STS_FATAL | STS_PCD | STS_ERR | STS_INT)
 
 /*-------------------------------------------------------------------------*/
@@ -305,6 +310,7 @@ static void end_unlink_async(struct ehci_hcd *ehci);
 static void ehci_work(struct ehci_hcd *ehci);
 
 #include "ehci-hub.c"
+#include "ehci-lpm.c"
 #include "ehci-mem.c"
 #include "ehci-q.c"
 #include "ehci-sched.c"
@@ -604,6 +610,17 @@ static int ehci_init(struct usb_hcd *hcd)
 		default:	BUG();
 		}
 	}
+	if (HCC_LPM(hcc_params)) {
+		/* support link power management EHCI 1.1 addendum */
+		ehci_dbg(ehci, "support lpm\n");
+		ehci->has_lpm = 1;
+		if (hird > 0xf) {
+			ehci_dbg(ehci, "hird %d invalid, use default 0",
+			hird);
+			hird = 0;
+		}
+		temp |= hird << 24;
+	}
 	ehci->command = temp;
 
 	/* Accept arbitrarily long scatter-gather lists */
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index e7d3d8def28..8a28dae8a37 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -790,6 +790,11 @@ static int ehci_hub_control (
 					  status_reg);
 			break;
 		case USB_PORT_FEAT_C_CONNECTION:
+			if (ehci->has_lpm) {
+				/* clear PORTSC bits on disconnect */
+				temp &= ~PORT_LPM;
+				temp &= ~PORT_DEV_ADDR;
+			}
 			ehci_writel(ehci, (temp & ~PORT_RWC_BITS) | PORT_CSC,
 					status_reg);
 			break;
diff --git a/drivers/usb/host/ehci-lpm.c b/drivers/usb/host/ehci-lpm.c
new file mode 100644
index 00000000000..b4d4d63c13e
--- /dev/null
+++ b/drivers/usb/host/ehci-lpm.c
@@ -0,0 +1,83 @@
+/* ehci-lpm.c EHCI HCD LPM support code
+ * Copyright (c) 2008 - 2010,  Intel Corporation.
+ * Author: Jacob Pan <jacob.jun.pan@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+/* this file is part of ehci-hcd.c */
+static int ehci_lpm_set_da(struct ehci_hcd *ehci, int dev_addr, int port_num)
+{
+	u32 __iomem portsc;
+
+	ehci_dbg(ehci, "set dev address %d for port %d\n", dev_addr, port_num);
+	if (port_num > HCS_N_PORTS(ehci->hcs_params)) {
+		ehci_dbg(ehci, "invalid port number %d\n", port_num);
+		return -ENODEV;
+	}
+	portsc = ehci_readl(ehci, &ehci->regs->port_status[port_num-1]);
+	portsc &= ~PORT_DEV_ADDR;
+	portsc |= dev_addr<<25;
+	ehci_writel(ehci, portsc, &ehci->regs->port_status[port_num-1]);
+	return 0;
+}
+
+/*
+ * this function is used to check if the device support LPM
+ * if yes, mark the PORTSC register with PORT_LPM bit
+ */
+static int ehci_lpm_check(struct ehci_hcd *ehci, int port)
+{
+	u32 __iomem	*portsc ;
+	u32 val32;
+	int retval;
+
+	portsc = &ehci->regs->port_status[port-1];
+	val32 = ehci_readl(ehci, portsc);
+	if (!(val32 & PORT_DEV_ADDR)) {
+		ehci_dbg(ehci, "LPM: no device attached\n");
+		return -ENODEV;
+	}
+	val32 |= PORT_LPM;
+	ehci_writel(ehci, val32, portsc);
+	msleep(5);
+	val32 |= PORT_SUSPEND;
+	ehci_dbg(ehci, "Sending LPM 0x%08x to port %d\n", val32, port);
+	ehci_writel(ehci, val32, portsc);
+	/* wait for ACK */
+	msleep(10);
+	retval = handshake(ehci, &ehci->regs->port_status[port-1], PORT_SSTS,
+			PORTSC_SUSPEND_STS_ACK, 125);
+	dbg_port(ehci, "LPM", port, val32);
+	if (retval != -ETIMEDOUT) {
+		ehci_dbg(ehci, "LPM: device ACK for LPM\n");
+		val32 |= PORT_LPM;
+		/*
+		 * now device should be in L1 sleep, let's wake up the device
+		 * so that we can complete enumeration.
+		 */
+		ehci_writel(ehci, val32, portsc);
+		msleep(10);
+		val32 |= PORT_RESUME;
+		ehci_writel(ehci, val32, portsc);
+	} else {
+		ehci_dbg(ehci, "LPM: device does not ACK, disable LPM %d\n",
+			retval);
+		val32 &= ~PORT_LPM;
+		retval = -ETIMEDOUT;
+		ehci_writel(ehci, val32, portsc);
+	}
+
+	return retval;
+}
diff --git a/drivers/usb/host/ehci-pci.c b/drivers/usb/host/ehci-pci.c
index d43d176161a..a307d550bda 100644
--- a/drivers/usb/host/ehci-pci.c
+++ b/drivers/usb/host/ehci-pci.c
@@ -361,6 +361,22 @@ static int ehci_pci_resume(struct usb_hcd *hcd, bool hibernated)
 }
 #endif
 
+static int ehci_update_device(struct usb_hcd *hcd, struct usb_device *udev)
+{
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	int rc = 0;
+
+	if (!udev->parent) /* udev is root hub itself, impossible */
+		rc = -1;
+	/* we only support lpm device connected to root hub yet */
+	if (ehci->has_lpm && !udev->parent->parent) {
+		rc = ehci_lpm_set_da(ehci, udev->devnum, udev->portnum);
+		if (!rc)
+			rc = ehci_lpm_check(ehci, udev->portnum);
+	}
+	return rc;
+}
+
 static const struct hc_driver ehci_pci_hc_driver = {
 	.description =		hcd_name,
 	.product_desc =		"EHCI Host Controller",
@@ -407,6 +423,11 @@ static const struct hc_driver ehci_pci_hc_driver = {
 	.relinquish_port =	ehci_relinquish_port,
 	.port_handed_over =	ehci_port_handed_over,
 
+	/*
+	 * call back when device connected and addressed
+	 */
+	.update_device =	ehci_update_device,
+
 	.clear_tt_buffer_complete	= ehci_clear_tt_buffer_complete,
 };
 
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index bfaac164636..21f30a0c3d2 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -140,7 +140,7 @@ struct ehci_hcd {			/* one per controller */
 	#define OHCI_HCCTRL_LEN         0x4
 	__hc32			*ohci_hcctrl_reg;
 	unsigned		has_hostpc:1;
-
+	unsigned		has_lpm:1;  /* support link power management */
 	u8			sbrn;		/* packed release number */
 
 	/* irq statistics */