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>

diff --git a/drivers/usb/host/pci-quirks.c b/drivers/usb/host/pci-quirks.c
index 9b166d7..e300509 100644 (file)
--- a/drivers/usb/host/pci-quirks.c
+++ b/drivers/usb/host/pci-quirks.c
@@ -503,14 +503,70 @@ static void __devinit quirk_usb_handoff_ohci(struct pci_dev *pdev)
        iounmap(base);
 }
 
+static void __devinit ehci_bios_handoff(struct pci_dev *pdev,
+                                       void __iomem *op_reg_base,
+                                       u32 cap, u8 offset)
+{
+       int msec, tried_handoff = 0;
+
+       if (cap & EHCI_USBLEGSUP_BIOS) {
+               dev_dbg(&pdev->dev, "EHCI: BIOS handoff\n");
+
+#if 0
+/* aleksey_gorelov@phoenix.com reports that some systems need SMI forced on,
+ * but that seems dubious in general (the BIOS left it off intentionally)
+ * 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
+                */
+               pci_read_config_dword(pdev, offset + EHCI_USBLEGCTLSTS, &val);
+               pci_write_config_dword(pdev, offset + EHCI_USBLEGCTLSTS,
+                                      val | EHCI_USBLEGCTLSTS_SOOE);
+#endif
+
+               /* some systems get upset if this semaphore is
+                * set for any other reason than forcing a BIOS
+                * handoff..
+                */
+               pci_write_config_byte(pdev, offset + 3, 1);
+       }
+
+       /* if boot firmware now owns EHCI, spin till it hands it over. */
+       msec = 1000;
+       while ((cap & EHCI_USBLEGSUP_BIOS) && (msec > 0)) {
+               tried_handoff = 1;
+               msleep(10);
+               msec -= 10;
+               pci_read_config_dword(pdev, offset, &cap);
+       }
+
+       if (cap & EHCI_USBLEGSUP_BIOS) {
+               /* 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);
+               pci_write_config_byte(pdev, offset + 2, 0);
+       }
+
+       /* just in case, always disable EHCI SMIs */
+       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 (tried_handoff)
+               writel(0, op_reg_base + EHCI_CONFIGFLAG);
+}
+
 static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
 {
-       int wait_time, delta;
        void __iomem *base, *op_reg_base;
-       u32     hcc_params, val;
+       u32     hcc_params, cap, val;
        u8      offset, cap_length;
-       int     count = 256/4;
-       int     tried_handoff = 0;
+       int     wait_time, delta, count = 256/4;
 
        if (!mmio_resource_enabled(pdev, 0))
                return;
@@ -529,77 +585,17 @@ static void __devinit quirk_usb_disable_ehci(struct pci_dev *pdev)
        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)) {
-                               dev_dbg(&pdev->dev, "EHCI: BIOS handoff\n");
-
-#if 0
-/* aleksey_gorelov@phoenix.com reports that some systems need SMI forced on,
- * but that seems dubious in general (the BIOS left it off intentionally)
- * 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
-                                */
-                               pci_read_config_dword(pdev,
-                                               offset + EHCI_USBLEGCTLSTS,
-                                               &val);
-                               pci_write_config_dword(pdev,
-                                               offset + EHCI_USBLEGCTLSTS,
-                                               val | EHCI_USBLEGCTLSTS_SOOE);
-#endif
-
-                               /* some systems get upset if this semaphore is
-                                * set for any other reason than forcing a BIOS
-                                * handoff..
-                                */
-                               pci_write_config_byte(pdev, offset + 3, 1);
-                       }
-
-                       /* if boot firmware now owns EHCI, spin till
-                        * it hands it over.
-                        */
-                       msec = 1000;
-                       while ((cap & EHCI_USBLEGSUP_BIOS) && (msec > 0)) {
-                               tried_handoff = 1;
-                               msleep(10);
-                               msec -= 10;
-                               pci_read_config_dword(pdev, offset, &cap);
-                       }
 
-                       if (cap & EHCI_USBLEGSUP_BIOS) {
-                               /* 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);
-                               pci_write_config_byte(pdev, offset + 2, 0);
-                       }
-
-                       /* just in case, always disable EHCI SMIs */
-                       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 (tried_handoff)
-                               writel(0, op_reg_base + EHCI_CONFIGFLAG);
+               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;
+                                "%02x\n", cap & 0xff);
                }
                offset = (cap >> 8) & 0xff;
        }