5 files changed, 101 insertions, 61 deletions

diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index f8fed163a23a..7e00ca095cea 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -546,7 +546,7 @@ static void ehci_work (struct ehci_hcd *ehci)
          */
         if (ehci->rh_state == EHCI_RH_RUNNING &&
                         (ehci->async->qh_next.ptr != NULL ||
-                         ehci->periodic_sched != 0))
+                         ehci->periodic_count != 0))
                 timer_action (ehci, TIMER_IO_WATCHDOG);
 }
 
diff --git a/drivers/usb/host/ehci-pci.c b/drivers/usb/host/ehci-pci.c
index 21e5f963f331..2cb7d370c4ef 100644
--- a/drivers/usb/host/ehci-pci.c
+++ b/drivers/usb/host/ehci-pci.c
@@ -104,10 +104,6 @@ static int ehci_pci_setup(struct usb_hcd *hcd)
                 break;
         case PCI_VENDOR_ID_INTEL:
                 ehci->fs_i_thresh = 1;
-                if (pdev->device == 0x27cc) {
-                        ehci->broken_periodic = 1;
-                        ehci_info(ehci, "using broken periodic workaround\n");
-                }
                 if (pdev->device == PCI_DEVICE_ID_INTEL_CE4100_USB)
                         hcd->has_tt = 1;
                 break;
diff --git a/drivers/usb/host/ehci-sched.c b/drivers/usb/host/ehci-sched.c
index 3429b8a33c58..f5c15880c65a 100644
--- a/drivers/usb/host/ehci-sched.c
+++ b/drivers/usb/host/ehci-sched.c
@@ -481,67 +481,26 @@ static int tt_no_collision (
 
 static int enable_periodic (struct ehci_hcd *ehci)
 {
-        int     status;
-
-        if (ehci->periodic_sched++)
+        if (ehci->periodic_count++)
                 return 0;
 
-        /* did clearing PSE did take effect yet?
-         * takes effect only at frame boundaries...
-         */
-        status = handshake_on_error_set_halt(ehci, &ehci->regs->status,
-                                             STS_PSS, 0, 9 * 125);
-        if (status) {
-                usb_hc_died(ehci_to_hcd(ehci));
-                return status;
-        }
-
-        ehci->command |= CMD_PSE;
-        ehci_writel(ehci, ehci->command, &ehci->regs->command);
-        /* posted write ... PSS happens later */
+        /* Stop waiting to turn off the periodic schedule */
+        ehci->enabled_hrtimer_events &= ~BIT(EHCI_HRTIMER_DISABLE_PERIODIC);
 
-        /* make sure ehci_work scans these */
-        ehci->next_uframe = ehci_read_frame_index(ehci)
-                % (ehci->periodic_size << 3);
-        if (unlikely(ehci->broken_periodic))
-                ehci->last_periodic_enable = ktime_get_real();
+        /* Don't start the schedule until PSS is 0 */
+        ehci_poll_PSS(ehci);
         return 0;
 }
 
 static int disable_periodic (struct ehci_hcd *ehci)
 {
-        int     status;
-
-        if (--ehci->periodic_sched)
+        if (--ehci->periodic_count)
                 return 0;
 
-        if (unlikely(ehci->broken_periodic)) {
-                /* delay experimentally determined */
-                ktime_t safe = ktime_add_us(ehci->last_periodic_enable, 1000);
-                ktime_t now = ktime_get_real();
-                s64 delay = ktime_us_delta(safe, now);
-
-                if (unlikely(delay > 0))
-                        udelay(delay);
-        }
-
-        /* did setting PSE not take effect yet?
-         * takes effect only at frame boundaries...
-         */
-        status = handshake_on_error_set_halt(ehci, &ehci->regs->status,
-                                             STS_PSS, STS_PSS, 9 * 125);
-        if (status) {
-                usb_hc_died(ehci_to_hcd(ehci));
-                return status;
-        }
-
-        ehci->command &= ~CMD_PSE;
-        ehci_writel(ehci, ehci->command, &ehci->regs->command);
-        /* posted write ... */
-
-        free_cached_lists(ehci);
+        ehci->next_uframe = -1;         /* the periodic schedule is empty */
 
-        ehci->next_uframe = -1;
+        /* Don't turn off the schedule until PSS is 1 */
+        ehci_poll_PSS(ehci);
         return 0;
 }
 
@@ -650,8 +609,7 @@ static int qh_unlink_periodic(struct ehci_hcd *ehci, struct ehci_qh *qh)
         qh->qh_state = QH_STATE_UNLINK;
         qh->qh_next.ptr = NULL;
 
-        /* maybe turn off periodic schedule */
-        return disable_periodic(ehci);
+        return 0;
 }
 
 static void intr_deschedule (struct ehci_hcd *ehci, struct ehci_qh *qh)
@@ -706,6 +664,9 @@ static void intr_deschedule (struct ehci_hcd *ehci, struct ehci_qh *qh)
                         ehci_err(ehci, "can't reschedule qh %p, err %d\n",
                                         qh, rc);
         }
+
+        /* maybe turn off periodic schedule */
+        disable_periodic(ehci);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -2447,7 +2408,7 @@ restart:
 
                         /* assume completion callbacks modify the queue */
                         if (unlikely (modified)) {
-                                if (likely(ehci->periodic_sched > 0))
+                                if (likely(ehci->periodic_count > 0))
                                         goto restart;
                                 /* short-circuit this scan */
                                 now_uframe = clock;
@@ -2476,7 +2437,7 @@ restart:
                         unsigned        now;
 
                         if (ehci->rh_state < EHCI_RH_RUNNING
-                                        || ehci->periodic_sched == 0)
+                                        || ehci->periodic_count == 0)
                                 break;
                         ehci->next_uframe = now_uframe;
                         now = ehci_read_frame_index(ehci) & (mod - 1);
diff --git a/drivers/usb/host/ehci-timer.c b/drivers/usb/host/ehci-timer.c
index f6279e2883a8..ecd3296157c6 100644
--- a/drivers/usb/host/ehci-timer.c
+++ b/drivers/usb/host/ehci-timer.c
@@ -16,6 +16,28 @@
 
 /*-------------------------------------------------------------------------*/
 
+/* Set a bit in the USBCMD register */
+static void ehci_set_command_bit(struct ehci_hcd *ehci, u32 bit)
+{
+        ehci->command |= bit;
+        ehci_writel(ehci, ehci->command, &ehci->regs->command);
+
+        /* unblock posted write */
+        ehci_readl(ehci, &ehci->regs->command);
+}
+
+/* Clear a bit in the USBCMD register */
+static void ehci_clear_command_bit(struct ehci_hcd *ehci, u32 bit)
+{
+        ehci->command &= ~bit;
+        ehci_writel(ehci, ehci->command, &ehci->regs->command);
+
+        /* unblock posted write */
+        ehci_readl(ehci, &ehci->regs->command);
+}
+
+/*-------------------------------------------------------------------------*/
+
 /*
  * EHCI timer support...  Now using hrtimers.
  *
@@ -45,6 +67,8 @@
  * the event types indexed by enum ehci_hrtimer_event in ehci.h.
  */
 static unsigned event_delays_ns[] = {
+        1 * NSEC_PER_MSEC,      /* EHCI_HRTIMER_POLL_PSS */
+        10 * NSEC_PER_MSEC,     /* EHCI_HRTIMER_DISABLE_PERIODIC */
 };
 
 /* Enable a pending hrtimer event */
@@ -67,12 +91,68 @@ static void ehci_enable_event(struct ehci_hcd *ehci, unsigned event,
 }
 
 
+/* Poll the STS_PSS status bit; see when it agrees with CMD_PSE */
+static void ehci_poll_PSS(struct ehci_hcd *ehci)
+{
+        unsigned        actual, want;
+
+        /* Don't do anything if the controller isn't running (e.g., died) */
+        if (ehci->rh_state != EHCI_RH_RUNNING)
+                return;
+
+        want = (ehci->command & CMD_PSE) ? STS_PSS : 0;
+        actual = ehci_readl(ehci, &ehci->regs->status) & STS_PSS;
+
+        if (want != actual) {
+
+                /* Poll again later, but give up after about 20 ms */
+                if (ehci->PSS_poll_count++ < 20) {
+                        ehci_enable_event(ehci, EHCI_HRTIMER_POLL_PSS, true);
+                        return;
+                }
+                ehci_warn(ehci, "Waited too long for the periodic schedule status, giving up\n");
+        }
+        ehci->PSS_poll_count = 0;
+
+        /* The status is up-to-date; restart or stop the schedule as needed */
+        if (want == 0) {        /* Stopped */
+                free_cached_lists(ehci);
+                if (ehci->periodic_count > 0) {
+
+                        /* make sure ehci_work scans these */
+                        ehci->next_uframe = ehci_read_frame_index(ehci)
+                                        & ((ehci->periodic_size << 3) - 1);
+                        ehci_set_command_bit(ehci, CMD_PSE);
+                }
+
+        } else {                /* Running */
+                if (ehci->periodic_count == 0) {
+
+                        /* Turn off the schedule after a while */
+                        ehci_enable_event(ehci, EHCI_HRTIMER_DISABLE_PERIODIC,
+                                        true);
+                }
+        }
+}
+
+/* Turn off the periodic schedule after a brief delay */
+static void ehci_disable_PSE(struct ehci_hcd *ehci)
+{
+        ehci_clear_command_bit(ehci, CMD_PSE);
+
+        /* Poll to see when it actually stops */
+        ehci_enable_event(ehci, EHCI_HRTIMER_POLL_PSS, true);
+}
+
+
 /*
  * Handler functions for the hrtimer event types.
  * Keep this array in the same order as the event types indexed by
  * enum ehci_hrtimer_event in ehci.h.
  */
 static void (*event_handlers[])(struct ehci_hcd *) = {
+        ehci_poll_PSS,                  /* EHCI_HRTIMER_POLL_PSS */
+        ehci_disable_PSE,               /* EHCI_HRTIMER_DISABLE_PERIODIC */
 };
 
 static enum hrtimer_restart ehci_hrtimer_func(struct hrtimer *t)
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index 070be83028d5..da2e0ab23850 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -79,6 +79,8 @@ enum ehci_rh_state {
  * ehci-timer.c) in parallel with this list.
  */
 enum ehci_hrtimer_event {
+        EHCI_HRTIMER_POLL_PSS,          /* Poll for periodic schedule off */
+        EHCI_HRTIMER_DISABLE_PERIODIC,  /* Wait to disable periodic sched */
         EHCI_HRTIMER_NUM_EVENTS         /* Must come last */
 };
 #define EHCI_HRTIMER_NO_EVENT   99
@@ -90,6 +92,8 @@ struct ehci_hcd {			/* one per controller */
         ktime_t                 hr_timeouts[EHCI_HRTIMER_NUM_EVENTS];
         struct hrtimer          hrtimer;
 
+        int                     PSS_poll_count;
+
         /* glue to PCI and HCD framework */
         struct ehci_caps __iomem *caps;
         struct ehci_regs __iomem *regs;
@@ -116,7 +120,7 @@ struct ehci_hcd {			/* one per controller */
 
         union ehci_shadow       *pshadow;       /* mirror hw periodic table */
         int                     next_uframe;    /* scan periodic, start here */
-        unsigned                periodic_sched; /* periodic activity count */
+        unsigned                periodic_count; /* periodic activity count */
         unsigned                uframe_periodic_max; /* max periodic time per uframe */
 
 
@@ -165,7 +169,6 @@ struct ehci_hcd {			/* one per controller */
         unsigned                big_endian_capbase:1;
         unsigned                has_amcc_usb23:1;
         unsigned                need_io_watchdog:1;
-        unsigned                broken_periodic:1;
         unsigned                amd_pll_fix:1;
         unsigned                fs_i_thresh:1;  /* Intel iso scheduling */
         unsigned                use_dummy_qh:1; /* AMD Frame List table quirk*/