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>

1 files changed, 66 insertions, 70 deletions

diff --git a/drivers/usb/host/pci-quirks.c b/drivers/usb/host/pci-quirks.c
index 9b166d70ae91..e300509faa61 100644
--- 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;
         }