ehci: pci quirk cleanup

Factor the handoff code out from quirk_usb_disable_ehci
Signed-off-by: Andy Ross <andy.ross@windriver.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>

drivers/usb/host/pci-quirks.c

@@ -503,39 +503,13 @@ static void __devinit quirk_usb_handoff_ohci(struct pci_dev *pdev)
 	iounmap(base);
 }
 
-static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
+static void __devinit ehci_bios_handoff(struct pci_dev *pdev,
+					void __iomem *op_reg_base,
+					u32 cap, u8 offset)
 {
-	int wait_time, delta;
-	void __iomem *base, *op_reg_base;
-	u32	hcc_params, val;
-	u8	offset, cap_length;
-	int	count = 256/4;
-	int	tried_handoff = 0;
-
-	if (!mmio_resource_enabled(pdev, 0))
-		return;
-
-	base = pci_ioremap_bar(pdev, 0);
-	if (base == NULL)
-		return;
+	int msec, tried_handoff = 0;
 
-	cap_length = readb(base);
-	op_reg_base = base + cap_length;
-
-	/* EHCI 0.96 and later may have "extended capabilities"
-	 * spec section 5.1 explains the bios handoff, e.g. for
-	 * booting from USB disk or using a usb keyboard
-	 */
-	hcc_params = readl(base + EHCI_HCC_PARAMS);
-	offset = (hcc_params >> 8) & 0xff;
-	while (offset && --count) {
-		u32		cap;
-		int		msec;
-
-		pci_read_config_dword(pdev, offset, &cap);
-		switch (cap & 0xff) {
-		case 1:			/* BIOS/SMM/... handoff support */
-			if ((cap & EHCI_USBLEGSUP_BIOS)) {
+	if (cap & EHCI_USBLEGSUP_BIOS) {
 		dev_dbg(&pdev->dev, "EHCI: BIOS handoff\n");
 
 #if 0
@@ -544,14 +518,11 @@ static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
  * and is known to prevent some systems from booting.  so we won't do this
  * unless maybe we can determine when we're on a system that needs SMI forced.
  */
-				/* BIOS workaround (?): be sure the
-				 * pre-Linux code receives the SMI
+		/* BIOS workaround (?): be sure the pre-Linux code
+		 * receives the SMI
 		 */
-				pci_read_config_dword(pdev,
-						offset + EHCI_USBLEGCTLSTS,
-						&val);
-				pci_write_config_dword(pdev,
-						offset + EHCI_USBLEGCTLSTS,
+		pci_read_config_dword(pdev, offset + EHCI_USBLEGCTLSTS, &val);
+		pci_write_config_dword(pdev, offset + EHCI_USBLEGCTLSTS,
 				       val | EHCI_USBLEGCTLSTS_SOOE);
 #endif
 
@@ -562,9 +533,7 @@ static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
 		pci_write_config_byte(pdev, offset + 3, 1);
 	}
 
-			/* if boot firmware now owns EHCI, spin till
-			 * it hands it over.
-			 */
+	/* if boot firmware now owns EHCI, spin till it hands it over. */
 	msec = 1000;
 	while ((cap & EHCI_USBLEGSUP_BIOS) && (msec > 0)) {
 		tried_handoff = 1;
@@ -574,8 +543,8 @@ static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
 	}
 
 	if (cap & EHCI_USBLEGSUP_BIOS) {
-				/* well, possibly buggy BIOS... try to shut
-				 * it down, and hope nothing goes too wrong
+		/* well, possibly buggy BIOS... try to shut it down,
+		 * and hope nothing goes too wrong
 		 */
 		dev_warn(&pdev->dev, "EHCI: BIOS handoff failed"
 			 " (BIOS bug?) %08x\n", cap);
@@ -583,23 +552,50 @@ static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
 	}
 
 	/* just in case, always disable EHCI SMIs */
-			pci_write_config_dword(pdev,
-					offset + EHCI_USBLEGCTLSTS,
-					0);
+	pci_write_config_dword(pdev, offset + EHCI_USBLEGCTLSTS, 0);
 
-			/* If the BIOS ever owned the controller then we
-			 * can't expect any power sessions to remain intact.
+	/* If the BIOS ever owned the controller then we can't expect
+	 * any power sessions to remain intact.
 	 */
 	if (tried_handoff)
 		writel(0, op_reg_base + EHCI_CONFIGFLAG);
+}
+
+static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
+{
+	void __iomem *base, *op_reg_base;
+	u32	hcc_params, cap, val;
+	u8	offset, cap_length;
+	int	wait_time, delta, count = 256/4;
+
+	if (!mmio_resource_enabled(pdev, 0))
+		return;
+
+	base = pci_ioremap_bar(pdev, 0);
+	if (base == NULL)
+		return;
+
+	cap_length = readb(base);
+	op_reg_base = base + cap_length;
+
+	/* EHCI 0.96 and later may have "extended capabilities"
+	 * spec section 5.1 explains the bios handoff, e.g. for
+	 * booting from USB disk or using a usb keyboard
+	 */
+	hcc_params = readl(base + EHCI_HCC_PARAMS);
+	offset = (hcc_params >> 8) & 0xff;
+	while (offset && --count) {
+		pci_read_config_dword(pdev, offset, &cap);
+
+		switch (cap & 0xff) {
+		case 1:
+			ehci_bios_handoff(pdev, op_reg_base, cap, offset);
 			break;
-		case 0:			/* illegal reserved capability */
-			cap = 0;
-			/* FALLTHROUGH */
+		case 0: /* Illegal reserved cap, set cap=0 so we exit */
+			cap = 0; /* then fallthrough... */
 		default:
 			dev_warn(&pdev->dev, "EHCI: unrecognized capability "
 				 "%02x\n", cap & 0xff);
-			break;
 		}
 		offset = (cap >> 8) & 0xff;
 	}