diff --git a/drv/stm32h7-qspi/src/lib.rs b/drv/stm32h7-qspi/src/lib.rs
index cedaae654d..581d322907 100644
--- a/drv/stm32h7-qspi/src/lib.rs
+++ b/drv/stm32h7-qspi/src/lib.rs
@@ -167,6 +167,9 @@ impl Qspi {
     }
 
     /// Helper for error paths.
+    ///
+    /// Disables FIFO Threshold, Transfer Complete, Transfer Error, and Timeout
+    /// interrupts.
     fn disable_all_interrupts(&self) {
         self.reg.cr.modify(|_, w| {
             w.ftie()
@@ -180,6 +183,51 @@ impl Qspi {
         });
     }
 
+    /// Returns the number of valid bytes being held in the FIFO queue if a
+    /// QSPI timeout or transfer error hasn't occurred.
+    #[inline(always)]
+    fn get_fifo_level(&self) -> Result<usize, QspiError> {
+        let sr = self.reg.sr.read();
+
+        // Check timeout bit.
+        if sr.tof().bit_is_set() {
+            // Clear timeout bit and return.
+            self.reg.fcr.modify(|_, w| w.ctof().set_bit());
+            return Err(QspiError::Timeout);
+        }
+        // Check transfer error bit.
+        if sr.tef().bit_is_set() {
+            // Clear transfer error bit and return.
+            self.reg.fcr.modify(|_, w| w.ctef().set_bit());
+            return Err(QspiError::TransferError);
+        }
+        // Re-enable transfer error and timeout interrupts to make sure we
+        // catch any future errors.
+        self.reg
+            .cr
+            .modify(|_, w| w.teie().set_bit().toie().set_bit());
+        Ok(usize::from(sr.flevel().bits()))
+    }
+
+    /// Wait for the Transfer Complete flag to get set.
+    ///
+    /// Note: this function returning does not guarantee that the BUSY flag is
+    /// clear.
+    #[inline(always)]
+    fn wait_for_transfer_complete(&self) {
+        // Disable FIFO threshold interrupt, and enable transfer complete
+        // interrupts.
+        self.reg
+            .cr
+            .modify(|_, w| w.ftie().clear_bit().tcie().set_bit());
+        while self.transfer_not_complete() {
+            // Unmask our interrupt.
+            sys_irq_control(self.interrupt, true);
+            // And wait for it to arrive.
+            sys_recv_notification(self.interrupt);
+        }
+    }
+
     fn write_impl(
         &self,
         command: Command,
@@ -187,9 +235,8 @@ impl Qspi {
         data: &[u8],
     ) -> Result<(), QspiError> {
         let result = self.write_impl_inner(command, addr, data);
-        if result.is_err() {
-            self.disable_all_interrupts();
-        }
+        // Clean up by disabling our interrupt sources.
+        self.disable_all_interrupts();
         result
     }
 
@@ -238,25 +285,11 @@ impl Qspi {
         // off the front.
         let mut data = data;
         while !data.is_empty() {
-            let sr = self.reg.sr.read();
-
-            if sr.tof().bit_is_set() {
-                self.reg.fcr.modify(|_, w| w.ctof().set_bit());
-                return Err(QspiError::Timeout);
-            }
-            if sr.tef().bit_is_set() {
-                self.reg.fcr.modify(|_, w| w.ctef().set_bit());
-                return Err(QspiError::TransferError);
-            }
-
-            // Make sure our errors are enabled
-            self.reg
-                .cr
-                .modify(|_, w| w.teie().set_bit().toie().set_bit());
+            // Check for any errors
+            let fl = self.get_fifo_level()?;
 
             // How much space is in the FIFO?
-            let fl = usize::from(sr.flevel().bits());
-            let ffree = FIFO_SIZE - fl;
+            let ffree = FIFO_SIZE.saturating_sub(fl);
             if ffree >= FIFO_THRESH.min(data.len()) {
                 // Calculate the write size. Note that this may be bigger than
                 // the threshold used above above. We'll opportunistically
@@ -293,18 +326,8 @@ impl Qspi {
         }
 
         // We're now interested in transfer complete, not FIFO ready.
-        self.reg
-            .cr
-            .modify(|_, w| w.ftie().clear_bit().tcie().set_bit());
-        while self.transfer_not_complete() {
-            // Unmask our interrupt.
-            sys_irq_control(self.interrupt, true);
-            // And wait for it to arrive.
-            sys_recv_notification(self.interrupt);
-        }
-        self.reg.cr.modify(|_, w| {
-            w.tcie().clear_bit().teie().clear_bit().toie().clear_bit()
-        });
+        self.wait_for_transfer_complete();
+
         Ok(())
     }
 
@@ -315,9 +338,8 @@ impl Qspi {
         out: &mut [u8],
     ) -> Result<(), QspiError> {
         let result = self.read_impl_inner(command, addr, out);
-        if result.is_err() {
-            self.disable_all_interrupts();
-        }
+        // Clean up by disabling our interrupt sources.
+        self.disable_all_interrupts();
         result
     }
 
@@ -375,22 +397,10 @@ impl Qspi {
         // perform transfers.
         let mut out = out;
         while !out.is_empty() {
-            let sr = self.reg.sr.read();
+            // Get the FIFO level if no errors have occurred.
+            let fl = self.get_fifo_level()?;
 
-            if sr.tof().bit_is_set() {
-                self.reg.fcr.modify(|_, w| w.ctof().set_bit());
-                return Err(QspiError::Timeout);
-            }
-            if sr.tef().bit_is_set() {
-                self.reg.fcr.modify(|_, w| w.ctef().set_bit());
-                return Err(QspiError::TransferError);
-            }
-            // Make sure our errors are enabled
-            self.reg
-                .cr
-                .modify(|_, w| w.teie().set_bit().toie().set_bit());
             // Is there enough to read that we want to bother with it?
-            let fl = usize::from(sr.flevel().bits());
             if fl < FIFO_THRESH.min(out.len()) {
                 // Nope! Let's wait for more bytes.
 
@@ -438,27 +448,8 @@ impl Qspi {
         // necessarily imply the BUSY flag is clear, but since commands are
         // issued into a FIFO, we can issue the next command even while BUSY is
         // set, it appears.
-        self.reg
-            .cr
-            .modify(|_, w| w.ftie().clear_bit().tcie().set_bit());
-        while self.transfer_not_complete() {
-            // Unmask our interrupt.
-            sys_irq_control(self.interrupt, true);
-            // And wait for it to arrive.
-            sys_recv_notification(self.interrupt);
-        }
+        self.wait_for_transfer_complete();
 
-        // Clean up by disabling our interrupt sources.
-        self.reg.cr.modify(|_, w| {
-            w.ftie()
-                .clear_bit()
-                .tcie()
-                .clear_bit()
-                .teie()
-                .clear_bit()
-                .toie()
-                .clear_bit()
-        });
         Ok(())
     }