1 files changed, 59 insertions, 63 deletions

diff --git a/usbh/src/pipe.rs b/usbh/src/pipe.rs
index f442873..c6f2023 100644
--- a/usbh/src/pipe.rs
+++ b/usbh/src/pipe.rs
@@ -290,9 +290,9 @@ impl Pipe<'_, '_> {
         }
     }
 
-    // This and `dispatch_result` are the only function that calls
-    // `millis`. If we can make this just take the current timestamp,
-    // we can make this non-blocking.
+    // This is the only function that calls `millis`. If we can make
+    // this just take the current timestamp, we can make this
+    // non-blocking.
     pub(crate) fn dispatch_retries(
         &mut self,
         token: USBToken,
@@ -304,24 +304,37 @@ impl Pipe<'_, '_> {
         trace!("initial regs");
         self.log_regs();
 
+        self.dispatch_packet(token);
+
         let until = millis() + USB_TIMEOUT;
-        let mut last_result: Result<(), PipeErr> = Err(PipeErr::SWTimeout);
+        let mut last_err = PipeErr::SWTimeout;
         let mut naks = 0;
-        while naks <= retries {
-            trace!("p{}: dispatch {:?} retry {}", self.num, token, naks);
-
-            self.dispatch_packet(token);
-            last_result = self.dispatch_result(token, until, millis);
-            match last_result {
-                Ok(_) => return Ok(()),
-                Err(PipeErr::DataToggle) => self.dtgl(),
-                Err(PipeErr::SWTimeout) => break,
-                Err(PipeErr::Stall) => break,
-                Err(_) => naks += 1,
+        while millis() < until {
+            match self.dispatch_result(token) {
+                Ok(true) => return Ok(()),
+                Ok(false) => continue,
+
+                Err(e) => {
+                    last_err = e;
+                    match last_err {
+                        PipeErr::DataToggle => self.dtgl(),
+                        PipeErr::Stall => break,
+                        _ => {
+                            naks += 1;
+                            if naks > retries {
+                                break;
+                            } else {
+                                self.dispatch_packet(token);
+                            }
+                        }
+                    }
+                }
             }
         }
 
-        last_result
+        trace!("dispatch err: {:?}", last_err);
+        self.log_regs();
+        Err(last_err)
     }
 
     fn dispatch_packet(&mut self, token: USBToken) {
@@ -343,54 +356,37 @@ impl Pipe<'_, '_> {
         self.regs.statusclr.write(|w| w.pfreeze().set_bit());
     }
 
-    fn dispatch_result(
-        &mut self,
-        token: USBToken,
-        until: usize,
-        millis: &dyn Fn() -> usize,
-    ) -> Result<(), PipeErr> {
-        while millis() <= until {
-            if self.is_transfer_complete(token)? {
-                return Ok(());
-            } else if self.regs.intflag.read().stall().bit_is_set() {
-                trace!("stall");
-                self.log_regs();
-                self.regs.intflag.write(|w| w.stall().set_bit());
-                return Err(PipeErr::Stall);
-            } else if self.regs.intflag.read().trfail().bit_is_set() {
-                trace!("trfail");
-                self.log_regs();
-                self.regs.intflag.write(|w| w.trfail().set_bit());
-                return Err(PipeErr::TransferFail);
-            } else if self.desc.bank0.status_bk.read().errorflow().bit_is_set() {
-                trace!("errorflow");
-                self.log_regs();
-                self.desc
-                    .bank0
-                    .status_bk
-                    .write(|w| w.errorflow().clear_bit());
-                return Err(PipeErr::Flow);
-            } else if self.desc.bank0.status_pipe.read().touter().bit_is_set() {
-                trace!("touter");
-                self.log_regs();
-                self.desc
-                    .bank0
-                    .status_pipe
-                    .write(|w| w.touter().clear_bit());
-                return Err(PipeErr::HWTimeout);
-            } else if self.desc.bank0.status_pipe.read().dtgler().bit_is_set() {
-                trace!("dtgler");
-                self.log_regs();
-                self.desc
-                    .bank0
-                    .status_pipe
-                    .write(|w| w.dtgler().clear_bit());
-                return Err(PipeErr::DataToggle);
-            }
+    fn dispatch_result(&mut self, token: USBToken) -> Result<bool, PipeErr> {
+        if self.is_transfer_complete(token)? {
+            Ok(true)
+        } else if self.regs.intflag.read().stall().bit_is_set() {
+            self.regs.intflag.write(|w| w.stall().set_bit());
+            Err(PipeErr::Stall)
+        } else if self.regs.intflag.read().trfail().bit_is_set() {
+            self.regs.intflag.write(|w| w.trfail().set_bit());
+            Err(PipeErr::TransferFail)
+        } else if self.desc.bank0.status_bk.read().errorflow().bit_is_set() {
+            self.desc
+                .bank0
+                .status_bk
+                .write(|w| w.errorflow().clear_bit());
+            Err(PipeErr::Flow)
+        } else if self.desc.bank0.status_pipe.read().touter().bit_is_set() {
+            self.desc
+                .bank0
+                .status_pipe
+                .write(|w| w.touter().clear_bit());
+            Err(PipeErr::HWTimeout)
+        } else if self.desc.bank0.status_pipe.read().dtgler().bit_is_set() {
+            self.desc
+                .bank0
+                .status_pipe
+                .write(|w| w.dtgler().clear_bit());
+            Err(PipeErr::DataToggle)
+        } else {
+            // Nothing wrong, but not done yet.
+            Ok(false)
         }
-        trace!("swtimeout");
-        self.log_regs();
-        Err(PipeErr::SWTimeout)
     }
 
     fn is_transfer_complete(&mut self, token: USBToken) -> Result<bool, PipeErr> {